Update to leaderboard 2

Author: joel-mb

carla_ad_agent/launch/carla_ad_agent.launch

@@ -10,11 +10,6 @@
   <arg name="Kd_longitudinal" default="0.515"/>
   <arg name="control_time_step" default="0.05"/>
 
-  <node pkg="carla_ad_agent" type="ad_agent.py" name="carla_ad_agent_$(arg role_name)" output="screen">
-    <param name="role_name" value="$(arg role_name)" />
-    <param name="avoid_risk" value="$(arg avoid_risk)" />
-  </node>
-
   <node pkg="carla_ad_agent" type="local_planner.py" name="local_planner_$(arg role_name)" output="screen">
     <param name="role_name" value="$(arg role_name)" />
     <param name="Kp_lateral" value="$(arg Kp_lateral)" />
@@ -25,5 +20,12 @@
     <param name="Kd_longitudinal" value="$(arg Kd_longitudinal)" />
     <param name="control_time_step" value="$(arg control_time_step)" />
   </node>
+
+  <node pkg="carla_ad_agent" type="ad_agent.py" name="carla_ad_agent_$(arg role_name)" output="screen">
+    <param name="role_name" value="$(arg role_name)" />
+    <param name="avoid_risk" value="$(arg avoid_risk)" />
+  </node>
+
+
 </launch>
 

carla_ad_agent/src/carla_ad_agent/ad_agent.py

@@ -50,7 +50,7 @@ def __init__(self):
         self._objects = {}
         self._lights_status = {}
         self._lights_info = {}
-        self._target_speed = 0.
+        self._target_speed = 8.33
 
         self.speed_command_publisher = self.new_publisher(
             Float64, "/carla/{}/speed_command".format(role_name),

carla_ad_agent/src/carla_ad_agent/local_planner.py

@@ -22,6 +22,7 @@
 from carla_ad_agent.misc import distance_vehicle
 
 from carla_msgs.msg import CarlaEgoVehicleControl  # pylint: disable=import-error
+from carla_waypoint_types.srv import ComputeRoute
 from nav_msgs.msg import Odometry, Path
 from std_msgs.msg import Float64
 from visualization_msgs.msg import Marker
@@ -67,6 +68,10 @@ def __init__(self):
         self._waypoints_queue = collections.deque(maxlen=20000)
         self._waypoint_buffer = collections.deque(maxlen=self._buffer_size)
 
+        self._compute_route_client = self.create_service_client(
+            '/carla_waypoint_publisher/compute_route',
+            ComputeRoute)
+
         # subscribers
         self._odometry_subscriber = self.new_subscription(
             Odometry,
@@ -115,6 +120,17 @@ def path_cb(self, path_msg):
             self._waypoints_queue.clear()
             self._waypoints_queue.extend([pose.pose for pose in path_msg.poses])
 
+    def plan_cb(self, path_msg):
+        request = get_service_request(ComputeRoute)
+        request.locations = [pose.pose.position for pose in path_msg.poses] 
+        response = self.call_service(self._compute_route_client, request)
+        with self.data_lock:
+            self._waypoint_buffer.clear()
+            self._waypoints_queue.clear()
+            self._waypoints_queue.extend([pose.pose for pose in response.route.poses])
+        
+        self._plan_publisher.publish(response.route)
+
     def pose_to_marker_msg(self, pose):
         marker_msg = Marker()
         marker_msg.type = 0
@@ -158,6 +174,7 @@ def run_step(self):
             # move using PID controllers
             control_msg = self._vehicle_controller.run_step(
                 self._target_speed, self._current_speed, self._current_pose, target_pose)
+            control_msg.header.stamp = ros_timestamp(self.get_time(), from_sec=True)
 
             # purge the queue of obsolete waypoints
             max_index = -1
@@ -176,6 +193,7 @@ def run_step(self):
 
     def emergency_stop(self):
         control_msg = CarlaEgoVehicleControl()
+        control_msg.header.stamp = ros_timestamp(self.get_time(), from_sec=True)
         control_msg.steer = 0.0
         control_msg.throttle = 0.0
         control_msg.brake = 1.0

carla_msgs

@@ -92,7 +92,10 @@ def _update_thread(self):
         """
         while not self.node.shutdown.is_set():
             time.sleep(ActorFactory.TIME_BETWEEN_UPDATES)
-            self.world.wait_for_tick()
+            try:
+                self.world.wait_for_tick(seconds=5)
+            except Exception as e:
+                self.node.logwarn(str(e))
             self.update_available_objects()
 
     def update_available_objects(self):
@@ -140,6 +143,9 @@ def update_actor_states(self, frame_id, timestamp):
         with self.lock:
             for actor_id in self.actors:
                 try:
+                    actor = self.actors[actor_id]
+                    if isinstance(actor, Sensor):
+                        self.node.loginfo(actor.__class__.__name__)
                     self.actors[actor_id].update(frame_id, timestamp)
                 except RuntimeError as e:
                     self.node.logwarn("Update actor {}({}) failed: {}".format(

carla_ros_bridge/src/carla_ros_bridge/actor_factory.py

@@ -156,9 +156,9 @@ def initialize_bridge(self, carla_world, params):
         self.get_blueprints_service = self.new_service(GetBlueprints, "/carla/get_blueprints",
                                                        self.get_blueprints, callback_group=self.callback_group)
 
-        self.carla_weather_subscriber = \
-            self.new_subscription(CarlaWeatherParameters, "/carla/weather_control",
-                                  self.on_weather_changed, qos_profile=10, callback_group=self.callback_group)
+        #self.carla_weather_subscriber = \
+        #    self.new_subscription(CarlaWeatherParameters, "/carla/weather_control",
+        #                          self.on_weather_changed, qos_profile=10, callback_group=self.callback_group)
 
     def spawn_object(self, req, response=None):
         response = roscomp.get_service_response(SpawnObject)
@@ -358,7 +358,7 @@ def destroy(self):
         self.status_publisher.destroy()
         self.destroy_service(self.spawn_object_service)
         self.destroy_service(self.destroy_object_service)
-        self.destroy_subscription(self.carla_weather_subscriber)
+        #self.destroy_subscription(self.carla_weather_subscriber)
         self.carla_control_queue.put(CarlaControl.STEP_ONCE)
 
         for uid in self._registered_actors:
@@ -411,19 +411,6 @@ def main(args=None):
             port=parameters['port'])
         carla_client.set_timeout(parameters['timeout'])
 
-        # check carla version
-        dist = pkg_resources.get_distribution("carla")
-        if LooseVersion(dist.version) != LooseVersion(CarlaRosBridge.CARLA_VERSION):
-            carla_bridge.logfatal("CARLA python module version {} required. Found: {}".format(
-                CarlaRosBridge.CARLA_VERSION, dist.version))
-            sys.exit(1)
-
-        if LooseVersion(carla_client.get_server_version()) != \
-           LooseVersion(carla_client.get_client_version()):
-            carla_bridge.logwarn(
-                "Version mismatch detected: You are trying to connect to a simulator that might be incompatible with this API. Client API version: {}. Simulator API version: {}"
-                .format(carla_client.get_client_version(),
-                        carla_client.get_server_version()))
 
         carla_world = carla_client.get_world()
 

carla_ros_bridge/src/carla_ros_bridge/bridge.py

@@ -7,8 +7,12 @@
 Classes to handle Carla imu sensor
 """
 
-from transforms3d.euler import euler2quat
+import math
 
+from sensor_msgs.msg import Imu
+
+from carla_ros_bridge.sensor import Sensor
+from transforms3d.euler import euler2quat, quat2euler
 import carla_common.transforms as trans
 
 from carla_ros_bridge.sensor import Sensor
@@ -69,7 +73,7 @@ def sensor_data_updated(self, carla_imu_measurement):
 
         # Carla uses a left-handed coordinate convention (X forward, Y right, Z up).
         # Here, these measurements are converted to the right-handed ROS convention
-        #  (X forward, Y left, Z up).
+        # (X forward, Y left, Z up).
         imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x
         imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
         imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z
@@ -78,11 +82,17 @@ def sensor_data_updated(self, carla_imu_measurement):
         imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y
         imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z
 
-        roll, pitch, yaw = trans.carla_rotation_to_RPY(carla_imu_measurement.transform.rotation)
-        quat = euler2quat(roll, pitch, yaw)
+        quat = euler2quat(math.sin(-carla_imu_measurement.compass), math.cos(-carla_imu_measurement.compass), 0.0)
         imu_msg.orientation.w = quat[0]
         imu_msg.orientation.x = quat[1]
         imu_msg.orientation.y = quat[2]
         imu_msg.orientation.z = quat[3]
 
+        roll, pitch, yaw = quat2euler([
+        imu_msg.orientation.w,
+        imu_msg.orientation.x,
+        imu_msg.orientation.y,
+        imu_msg.orientation.z])
+        compass = -math.atan2(roll, pitch) + 2*math.pi
+
         self.imu_publisher.publish(imu_msg)

carla_ros_bridge/src/carla_ros_bridge/imu.py

@@ -107,20 +107,16 @@ def __init__(self,  # pylint: disable=too-many-arguments
             self._tf_broadcaster = tf2_ros.TransformBroadcaster(node)
 
     def get_ros_transform(self, pose, timestamp):
-        if self.synchronous_mode:
-            if not self.relative_spawn_pose:
-                self.node.logwarn("{}: No relative spawn pose defined".format(self.get_prefix()))
-                return
-            pose = self.relative_spawn_pose
-            child_frame_id = self.get_prefix()
-            if self.parent is not None:
-                frame_id = self.parent.get_prefix()
-            else:
-                frame_id = "map"
-
+        if not self.relative_spawn_pose:
+            self.node.logwarn("{}: No relative spawn pose defined".format(self.get_prefix()))
+            return
+        pose = self.relative_spawn_pose
+        child_frame_id = self.get_prefix()
+        if self.parent is not None:
+            frame_id = self.parent.get_prefix()
         else:
-            child_frame_id = self.get_prefix()
             frame_id = "map"
+        self.node.loginfo("Haciendo relative tf: {}, {}".format(child_frame_id, frame_id))
 
         transform = tf2_ros.TransformStamped()
         transform.header.stamp = roscomp.ros_timestamp(sec=timestamp, from_sec=True)

carla_ros_bridge/src/carla_ros_bridge/sensor.py

@@ -13,7 +13,7 @@
 
 from carla_ros_bridge.pseudo_actor import PseudoActor
 
-from std_msgs.msg import Float32
+from carla_msgs.msg import CarlaSpeedometer
 
 
 class SpeedometerSensor(PseudoActor):
@@ -41,7 +41,7 @@ def __init__(self, uid, name, parent, node):
                                                 parent=parent,
                                                 node=node)
 
-        self.speedometer_publisher = node.new_publisher(Float32,
+        self.speedometer_publisher = node.new_publisher(CarlaSpeedometer,
                                                         self.get_topic_prefix(),
                                                         qos_profile=10)
 
@@ -79,5 +79,9 @@ def update(self, frame, timestamp):
             np.sin(pitch)
         ])
         speed = np.dot(vel_np, orientation)
+        
+        speed_msg = CarlaSpeedometer()
+        speed_msg.header = self.get_msg_header(timestamp=timestamp)
+        speed_msg.speed = speed
 
-        self.speedometer_publisher.publish(Float32(data=speed))
+        self.speedometer_publisher.publish(speed_msg)

carla_ros_bridge/src/carla_ros_bridge/speedometer_sensor.py

@@ -31,9 +31,10 @@
 from ros_compatibility.qos import QoSProfile, DurabilityPolicy
 
 from carla_msgs.msg import CarlaWorldInfo
-from carla_waypoint_types.srv import GetWaypoint, GetActorWaypoint
-from geometry_msgs.msg import PoseStamped
-from nav_msgs.msg import Path
+from carla_waypoint_types.srv import ComputeRoute, GetWaypoint, GetActorWaypoint
+
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
 
 
 class CarlaToRosWaypointConverter(CompatibleNode):
@@ -73,6 +74,12 @@ def __init__(self):
             '/carla_waypoint_publisher/{}/get_actor_waypoint'.format(self.role_name),
             self.get_actor_waypoint)
 
+        self.compute_route_service = self.new_service(
+            ComputeRoute,
+            '/carla_waypoint_publisher/compute_route',
+            self.compute_route
+        )
+
         # set initial goal
         self.goal = self.world.get_map().get_spawn_points()[0]
 
@@ -83,10 +90,6 @@ def __init__(self):
             self.on_goal,
             qos_profile=10)
 
-        # use callback to wait for ego vehicle
-        self.loginfo("Waiting for ego vehicle...")
-        self.on_tick = self.world.on_tick(self.find_ego_vehicle_actor)
-
     def destroy(self):
         """
         Destructor
@@ -133,6 +136,32 @@ def get_actor_waypoint(self, req, response=None):
             self.logwarn("get_actor_waypoint(): Actor {} not valid.".format(req.id))
         return response
 
+    def compute_route(self, req):
+        map_ = self.world.get_map()
+
+        dao = GlobalRoutePlannerDAO(map_, sampling_resolution=1)
+        grp = GlobalRoutePlanner(dao)
+        grp.setup()
+
+        path, previous_waypoint = [], None
+        for location in req.locations:
+            waypoint = map_.get_waypoint(trans.ros_point_to_carla_location(location))
+            if  previous_waypoint:
+                route_segment = grp.trace_route(previous_waypoint.transform.location, waypoint.transform.location)
+                path.extend(route_segment)
+
+            previous_waypoint = waypoint
+
+        response = get_service_response(ComputeRoute)
+        response.route.header.frame_id = "map"
+        response.route.header.stamp = ros_timestamp(self.get_time(), from_sec=True)
+        for wp in path:
+            pose = PoseStamped()
+            pose.pose = trans.carla_transform_to_ros_pose(wp[0].transform)
+            response.route.poses.append(pose)
+
+        return response
+
     def on_goal(self, goal):
         """
         Callback for /move_base_simple/goal