def get_ros_transform(self, transform=None):
"""
Function to provide the current ROS transform
:return: the ROS transfrom
:rtype: geometry_msgs.msg.TransformStamped
"""
tf_msg = TransformStamped()
tf_msg.header = self.get_msg_header("map")
tf_msg.child_frame_id = self.get_prefix()
if transform:
tf_msg.transform = trans.carla_transform_to_ros_transform(transform)
else:
tf_msg.transform = trans.carla_transform_to_ros_transform(
self.carla_actor.get_transform())
return tf_msg
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
else:
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 _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
# for camera we reorient it to look at the same axis as the opencv projection
# in order to get easy depth cloud for RGBD camera
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
rotation = t.transform.rotation
quat = [rotation.x, rotation.y, rotation.z, rotation.w]
quat_swap = tf.transformations.quaternion_from_matrix(
[[0, 0, 1, 0],
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
quat = tf.transformations.quaternion_multiply(quat, quat_swap)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.process_msg_fun('tf', t)
def _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
# for camera we reorient it to look at the same axis as the opencv projection
# in order to get easy depth cloud for RGBD camera
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
rotation = t.transform.rotation
quat = [rotation.x, rotation.y, rotation.z, rotation.w]
quat_swap = tf.transformations.quaternion_from_matrix([[0, 0, 1, 0],
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
quat = tf.transformations.quaternion_multiply(quat, quat_swap)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.process_msg_fun('tf', t)
def _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
# for camera we reorient it to look at the same axis as the opencv projection
# in order to get easy depth cloud for RGBD camera
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
rotation = t.transform.rotation
quat = [rotation.x, rotation.y, rotation.z, rotation.w]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
roll -= math.pi / 2.0
yaw -= math.pi / 2.0
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.process_msg_fun('tf', t)
def get_current_ros_transform(self):
"""
Override Function used to provide the current ROS transform
:return: the ROS transfrom of this Actor
:rtype: geometry-msgs.msg.Transform
"""
return trans.carla_transform_to_ros_transform(
self.carla_actor.get_transform())
def get_current_ros_transform(self):
"""
Override Function used to provide the current ROS transform.
In general sensors are also actors, therefore they contain a transform that is updated within each tick.
But the TF being published should exactly match the transform received by SensorData.
:return: the ROS transform of this actor
:rtype: geometry_msgs.msg.Transform
"""
return trans.carla_transform_to_ros_transform(self.current_sensor_data.transform)
def _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = self.cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
self.process_msg_fun('tf', t)
def process_msg(self, data, cur_time):
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = self.world_link
t.child_frame_id = "base_link"
t.transform = carla_transform_to_ros_transform(
carla_Transform(data.transform))
header = Header()
header.stamp = cur_time
header.frame_id = self.world_link
marker = get_vehicle_marker(
data, header=header, marker_id=0, is_player=True)
self.process_msg_fun(self.name, marker)
self.process_msg_fun('tf', t)
def process_msg(self, data, cur_time):
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = self.world_link
t.child_frame_id = "base_link"
t.transform = carla_transform_to_ros_transform(
carla_Transform(data.transform))
header = Header()
header.stamp = cur_time
header.frame_id = self.world_link
marker = get_vehicle_marker(data,
header=header,
marker_id=0,
is_player=True)
self.process_msg_fun(self.name, marker)
self.process_msg_fun('tf', t)
def update_marker_pose(object, base_marker):
"""
Update pose of a marker based on carla information
:param object: pb2 carla object
:param base_marker: marker to update pose
"""
ros_transform = carla_transform_to_ros_transform(
carla_Transform(object.transform))
base_marker.pose = ros_transform_to_pose(ros_transform)
base_marker.scale.x = object.bounding_box.extent.x * 2.0
base_marker.scale.y = object.bounding_box.extent.y * 2.0
base_marker.scale.z = object.bounding_box.extent.z * 2.0
base_marker.type = Marker.CUBE
def update_marker_pose(object, base_marker):
"""
Update pose of a marker based on carla information
:param object: pb2 carla object
:param base_marker: marker to update pose
"""
ros_transform = carla_transform_to_ros_transform(
carla_Transform(object.bounding_box.transform) *
carla_Transform(object.transform))
base_marker.pose = ros_transform_to_pose(ros_transform)
base_marker.scale.x = object.bounding_box.extent.x * 2.0
base_marker.scale.y = object.bounding_box.extent.y * 2.0
base_marker.scale.z = object.bounding_box.extent.z * 2.0
base_marker.type = Marker.CUBE
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
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 _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
# for some reasons lidar sends already rotated cloud,
# so it is need to ignore pitch and roll
r = t.transform.rotation
quat = [r.x, r.y, r.z, r.w]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.process_msg_fun('tf', t)
def _compute_transform(self, sensor_data, cur_time):
parent_frame_id = "base_link"
child_frame_id = self.name
t = TransformStamped()
t.header.stamp = cur_time
t.header.frame_id = parent_frame_id
t.child_frame_id = child_frame_id
t.transform = carla_transform_to_ros_transform(
self.carla_object.get_transform())
# for some reasons lidar sends already rotated cloud,
# so it is need to ignore pitch and roll
r = t.transform.rotation
quat = [r.x, r.y, r.z, r.w]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
self.process_msg_fun('tf', t)
