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
