def _update_synchronous_sensor(self, frame, timestamp):
while not self.next_data_expected_time or\
(not self.queue.empty() or
self.next_data_expected_time and
self.next_data_expected_time < timestamp):
while True:
try:
carla_sensor_data = self.queue.get(timeout=1.0)
if carla_sensor_data.frame == frame:
rospy.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_transform(
self.get_ros_transform(
trans.carla_transform_to_ros_transform(
carla_sensor_data.transform)))
self.sensor_data_updated(carla_sensor_data)
return
elif carla_sensor_data.frame < frame:
rospy.logwarn(
"{}({}): skipping old frame {}, expected {}".
format(self.__class__.__name__, self.get_id(),
carla_sensor_data.frame, frame))
except queue.Empty:
if not rospy.is_shutdown():
rospy.logwarn(
"{}({}): Expected Frame {} not received".format(
self.__class__.__name__, self.get_id(), frame))
return
def get_ros_transform(self,
transform=None,
frame_id=None,
child_frame_id=None):
"""
Function to provide the current ROS transform
:return: the ROS transfrom
:rtype: geometry_msgs.msg.TransformStamped
"""
tf_msg = TransformStamped()
if frame_id:
tf_msg.header = self.get_msg_header(frame_id)
else:
tf_msg.header = self.get_msg_header("map")
if child_frame_id:
tf_msg.child_frame_id = child_frame_id
else:
tf_msg.child_frame_id = self.get_prefix()
if transform:
tf_msg.transform = transform
else:
tf_msg.transform = trans.carla_transform_to_ros_transform(
self.carla_actor.get_transform())
return tf_msg
def get_current_ros_transform(self):
"""
Function to provide the current ROS pose
:return: the ROS pose of this actor
:rtype: geometry_msgs.msg.Pose
"""
return trans.carla_transform_to_ros_transform(
self.carla_actor.get_transform())
def _callback_sensor_data(self, carla_sensor_data):
"""
Callback function called whenever new sensor data is received
:param carla_sensor_data: carla sensor data object
:type carla_sensor_data: carla.SensorData
"""
if not rospy.is_shutdown():
if self.synchronous_mode:
if self.sensor_tick_time:
self.next_data_expected_time = carla_sensor_data.timestamp + \
float(self.sensor_tick_time)
self.queue.put(carla_sensor_data)
else:
self.publish_transform(self.get_ros_transform(
trans.carla_transform_to_ros_transform(carla_sensor_data.transform)))
self.sensor_data_updated(carla_sensor_data)
def _update_synchronous_event_sensor(self, frame):
while True:
try:
carla_sensor_data = self.queue.get(block=False)
if carla_sensor_data.frame != frame:
rospy.logwarn("{}({}): Received event for frame {}"
" (expected {}). Process it anyways.".format(
self.__class__.__name__, self.get_id(),
carla_sensor_data.frame, frame))
rospy.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_transform(
self.get_ros_transform(
trans.carla_transform_to_ros_transform(
carla_sensor_data.transform)))
self.sensor_data_updated(carla_sensor_data)
except queue.Empty:
return
