Fix timestamp synchronization for some sensors (#551)

When generating the header field for some sensors, the timestamp now is used. This is leading to race conditions and synchronization issues between some sensors. This PR fixes the timestamp by using the timestamp provided by the sensor data.

carla_ros_bridge/src/carla_ros_bridge/collision_sensor.py

@@ -67,7 +67,7 @@ def sensor_data_updated(self, collision_event):
         :type collision_event: carla.CollisionEvent
         """
         collision_msg = CarlaCollisionEvent()
-        collision_msg.header = self.get_msg_header()
+        collision_msg.header = self.get_msg_header(timestamp=collision_event.timestamp)
         collision_msg.other_actor_id = collision_event.other_actor.id
         collision_msg.normal_impulse.x = collision_event.normal_impulse.x
         collision_msg.normal_impulse.y = collision_event.normal_impulse.y

carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py

@@ -109,14 +109,14 @@ def get_marker_color(self):
         color.b = 0.0
         return color
 
-    def send_vehicle_msgs(self):
+    def send_vehicle_msgs(self, frame, timestamp):
         """
         send messages related to vehicle status
 
         :return:
         """
         vehicle_status = CarlaEgoVehicleStatus(
-            header=self.get_msg_header("map"))
+            header=self.get_msg_header("map", timestamp=timestamp))
         vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
         vehicle_status.acceleration.linear = self.get_current_ros_accel().linear
         vehicle_status.orientation = self.get_current_ros_pose().orientation
@@ -185,7 +185,7 @@ def update(self, frame, timestamp):
 
         :return:
         """
-        self.send_vehicle_msgs()
+        self.send_vehicle_msgs(frame, timestamp)
         super(EgoVehicle, self).update(frame, timestamp)
 
     def destroy(self):

carla_ros_bridge/src/carla_ros_bridge/lane_invasion_sensor.py

@@ -67,7 +67,7 @@ def sensor_data_updated(self, lane_invasion_event):
         :type lane_invasion_event: carla.LaneInvasionEvent
         """
         lane_invasion_msg = CarlaLaneInvasionEvent()
-        lane_invasion_msg.header = self.get_msg_header()
+        lane_invasion_msg.header = self.get_msg_header(timestamp=lane_invasion_event.timestamp)
         for marking in lane_invasion_event.crossed_lane_markings:
             lane_invasion_msg.crossed_lane_markings.append(marking.type)
         self.lane_invasion_publisher.publish(lane_invasion_msg)

carla_ros_bridge/src/carla_ros_bridge/lidar.py

@@ -138,7 +138,7 @@ def sensor_data_updated(self, carla_lidar_measurement):
         :param carla_lidar_measurement: carla semantic lidar measurement object
         :type carla_lidar_measurement: carla.SemanticLidarMeasurement
         """
-        header = self.get_msg_header()
+        header = self.get_msg_header(timestamp=carla_lidar_measurement.timestamp)
         fields = [
             PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
             PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),

carla_ros_bridge/src/carla_ros_bridge/marker_sensor.py

@@ -151,5 +151,5 @@ def update(self, frame, timestamp):
         marker_array_msg = MarkerArray()
         for actor in self.actor_list.values():
             if isinstance(actor, TrafficParticipant):
-                marker_array_msg.markers.append(actor.get_marker())
+                marker_array_msg.markers.append(actor.get_marker(timestamp))
         self.marker_publisher.publish(marker_array_msg)

carla_ros_bridge/src/carla_ros_bridge/object_sensor.py

@@ -71,7 +71,8 @@ def update(self, frame, timestamp):
         - tf global frame
         :return:
         """
-        ros_objects = ObjectArray(header=self.get_msg_header("map"))
+        ros_objects = ObjectArray()
+        ros_objects.header = self.get_msg_header(frame_id="map", timestamp=timestamp)
         for actor_id in self.actor_list.keys():
             # currently only Vehicles and Walkers are added to the object array
             if self.parent is None or self.parent.uid != actor_id:

carla_ros_bridge/src/carla_ros_bridge/odom_sensor.py

@@ -61,7 +61,7 @@ def update(self, frame, timestamp):
         """
         Function (override) to update this object.
         """
-        odometry = Odometry(header=self.parent.get_msg_header("map"))
+        odometry = Odometry(header=self.parent.get_msg_header("map", timestamp=timestamp))
         odometry.child_frame_id = self.parent.get_prefix()
         try:
             odometry.pose.pose = self.parent.get_current_ros_pose()

carla_ros_bridge/src/carla_ros_bridge/traffic_participant.py

@@ -123,14 +123,14 @@ def get_marker_pose(self):
         """
         return trans.carla_transform_to_ros_pose(self.carla_actor.get_transform())
 
-    def get_marker(self):
+    def get_marker(self, timestamp=None):
         """
         Helper function to create a ROS visualization_msgs.msg.Marker for the actor
 
         :return:
         visualization_msgs.msg.Marker
         """
-        marker = Marker(header=self.get_msg_header(frame_id="map"))
+        marker = Marker(header=self.get_msg_header(frame_id="map", timestamp=timestamp))
         marker.color = self.get_marker_color()
         marker.color.a = 0.3
         marker.id = self.get_id()