class SelfPoseListener(Node): def __init__(self): super().__init__("self_pose_listener") self.tf_buffer = Buffer() self._tf_listener = TransformListener(self.tf_buffer, self) self.self_pose = PoseStamped() def get_current_pose(self): try: tf = self.tf_buffer.lookup_transform("map", "base_link", rclpy.time.Time()) tf_time = self.tf_buffer.get_latest_common_time("map", "base_link") self.self_pose = SelfPoseListener.create_pose(tf_time, "map", tf) except LookupException as e: self.get_logger().warn( "Required transformation not found: `{}`".format(str(e))) return None @staticmethod def create_pose(time, frame_id, tf): pose = PoseStamped() pose.header.stamp = time.to_msg() pose.header.frame_id = frame_id pose.pose.position.x = tf.transform.translation.x pose.pose.position.y = tf.transform.translation.y pose.pose.position.z = tf.transform.translation.z pose.pose.orientation.x = tf.transform.rotation.x pose.pose.orientation.y = tf.transform.rotation.y pose.pose.orientation.z = tf.transform.rotation.z pose.pose.orientation.w = tf.transform.rotation.w return pose
class Tf2PoseNode(Node): def __init__(self, options): super().__init__("tf2pose") self._options = options self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self._pub_pose = self.create_publisher( PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1) self.timer = self.create_timer((1.0 / self._options.hz), self._on_timer) def _on_timer(self): try: tf = self.tf_buffer.lookup_transform(self._options.tf_from, self._options.tf_to, rclpy.time.Time()) time = self.tf_buffer.get_latest_common_time( self._options.tf_from, self._options.tf_to) pose = Tf2PoseNode.create_pose(time, self._options.tf_from, tf) self._pub_pose.publish(pose) except LookupException as e: print(e) @staticmethod def create_pose(time, frame_id, tf): pose = PoseStamped() pose.header.stamp = time.to_msg() pose.header.frame_id = frame_id pose.pose.position.x = tf.transform.translation.x pose.pose.position.y = tf.transform.translation.y pose.pose.position.z = tf.transform.translation.z pose.pose.orientation.x = tf.transform.rotation.x pose.pose.orientation.y = tf.transform.rotation.y pose.pose.orientation.z = tf.transform.rotation.z pose.pose.orientation.w = tf.transform.rotation.w return pose