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)