Merge pull request #42 from utiasDSL/feat-tf-pose-retrieval

Use tf to get the tool pose instead of current_pose topic

Author: danielsanjosepro

crisp_py/robot.py

@@ -20,6 +20,7 @@
 from crisp_py.robot_config import FrankaConfig, RobotConfig
 from crisp_py.utils.callback_monitor import CallbackMonitor
 from crisp_py.utils.geometry import Pose, Twist
+from crisp_py.utils.tf_pose import TfPose
 
 
 class Robot:
@@ -60,7 +61,10 @@ def __init__(
         if node is None:
             if not rclpy.ok():
                 rclpy.init()
-            self.node = rclpy.create_node(name, namespace=namespace)
+            self.node = rclpy.create_node(
+                name,
+                namespace=namespace,
+            )
         else:
             self.node = node
         self.config = robot_config if robot_config else FrankaConfig()
@@ -82,6 +86,7 @@ def __init__(
         self._target_joint = None
         self._target_wrench = None
         self._current_twist = None
+        self._tf_pose = None
 
         self._callback_monitor = CallbackMonitor(
             node=self.node,
@@ -97,15 +102,23 @@ def __init__(
         self._target_joint_publisher = self.node.create_publisher(
             JointState, self.config.target_joint_topic, qos_profile_system_default
         )
-        self.node.create_subscription(
-            PoseStamped,
-            self.config.current_pose_topic,
-            self._callback_monitor.monitor(
-                f"{namespace.capitalize()} Current Pose", self._callback_current_pose
-            ),
-            qos_profile_sensor_data,
-            callback_group=ReentrantCallbackGroup(),
-        )
+        if self.config.use_tf_pose:
+            self._tf_pose = TfPose(
+                self.node,
+                self.config.target_frame,
+                self.config.base_frame,
+                self.config.tf_retrieve_rate,
+            )
+        else:
+            self.node.create_subscription(
+                PoseStamped,
+                self.config.current_pose_topic,
+                self._callback_monitor.monitor(
+                    f"{namespace.capitalize()} Current Pose", self._callback_current_pose
+                ),
+                qos_profile_sensor_data,
+                callback_group=ReentrantCallbackGroup(),
+            )
         self.node.create_subscription(
             JointState,
             self.config.current_joint_topic,
@@ -126,13 +139,20 @@ def __init__(
             callback_group=ReentrantCallbackGroup(),
         )
 
-        self.node.create_timer(
-            1.0 / self.config.publish_frequency,
-            self._callback_monitor.monitor(
-                f"{namespace.capitalize()} Target Pose", self._callback_publish_target_pose
-            ),
-            ReentrantCallbackGroup(),
-        )
+        if not self.config.use_tf_pose:
+            self.node.create_timer(
+                1.0 / self.config.publish_frequency,
+                self._callback_monitor.monitor(
+                    f"{namespace.capitalize()} Target Pose", self._callback_publish_target_pose
+                ),
+                ReentrantCallbackGroup(),
+            )
+        else:
+            self.node.create_timer(
+                1.0 / self.config.publish_frequency,
+                self._callback_update_tf_pose,
+                ReentrantCallbackGroup(),
+            )
         self.node.create_timer(
             1.0 / self.config.publish_frequency,
             self._callback_monitor.monitor(
@@ -234,6 +254,13 @@ def end_effector_twist(self) -> Twist:
             )
         return self._current_twist.copy()
 
+    def _callback_update_tf_pose(self):
+        """Update the current pose from TF if TF pose is enabled."""
+        if self._tf_pose is not None and self._tf_pose.current_pose is not None:
+            self._current_pose = self._tf_pose.pose
+            if self._target_pose is None:
+                self._target_pose = self._current_pose.copy()
+
     def is_ready(self) -> bool:
         """Check if the robot is ready for operation.
 

crisp_py/robot_config.py

@@ -49,6 +49,9 @@ class RobotConfig:
     max_pose_delay: float = 1.0
     max_joint_delay: float = 1.0
 
+    use_tf_pose: bool = False
+    tf_retrieve_rate: float = 50.0
+
     def num_joints(self) -> int:
         """Returns the number of joints in the robot."""
         return len(self.joint_names)

crisp_py/utils/geometry.py

@@ -6,6 +6,7 @@
 from builtin_interfaces.msg import Time as TimeMsg
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from scipy.spatial.transform import Rotation
+from tf2_ros import TransformStamped
 
 
 @dataclass
@@ -51,6 +52,22 @@ def from_ros_msg(cls, msg: PoseStamped) -> "Pose":
         )
         return cls(position, orientation)
 
+    @classmethod
+    def from_transform_msg(cls, msg: TransformStamped) -> "Pose":
+        """Create Pose from ROS TransformStamped message."""
+        position = np.array(
+            [msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z]
+        )
+        orientation = Rotation.from_quat(
+            [
+                msg.transform.rotation.x,
+                msg.transform.rotation.y,
+                msg.transform.rotation.z,
+                msg.transform.rotation.w,
+            ]
+        )
+        return cls(position, orientation)
+
     def to_ros_msg(self, frame_id: str, stamp: TimeMsg) -> PoseStamped:
         """Convert Pose to ROS PoseStamped message."""
         msg = PoseStamped()

crisp_py/utils/tf_pose.py

@@ -0,0 +1,53 @@
+"""Util class to get a pose from the tf2 instead of using a dedicated topic."""
+
+from rclpy.node import Node
+from rclpy.time import Time
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+from crisp_py.utils.geometry import Pose
+
+
+class TfPose:
+    """Class to get a pose from the tf2 instead of using a dedicated topic."""
+
+    def __init__(
+        self, node: Node, target_frame: str, source_frame: str, retrieve_rate: float = 50.0
+    ):
+        """Initialize the TfPose class.
+
+        Args:
+            node (Node): The ROS2 node.
+            target_frame (str): The target frame to get the pose of.
+            source_frame (str): The source frame to get the pose relative to.
+            retrieve_rate (float, optional): The rate at which to retrieve the pose. Defaults to 50.0.
+        """
+        self.node = node
+        self.target_frame = target_frame
+        self.source_frame = source_frame
+        self.retrieve_rate = retrieve_rate
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self.node)
+
+        self.timer = self.node.create_timer(1.0 / self.retrieve_rate, self._callback_retrieve_pose)
+
+        self.current_pose: Pose | None = None
+
+    @property
+    def pose(self) -> Pose:
+        """Get the current pose."""
+        if self.current_pose is None:
+            raise ValueError("Pose has not been retrieved yet.")
+        return self.current_pose
+
+    def _callback_retrieve_pose(self):
+        """Retrieve the latest transform between source_frame and target_frame."""
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.source_frame, self.target_frame, Time()
+            )
+        except Exception as e:
+            self.node.get_logger().warn(f"Could not get transform: {e}", throttle_duration_sec=5.0)
+            return
+        self.current_pose = Pose.from_transform_msg(transform)

examples/17_tf_pose.py

@@ -0,0 +1,31 @@
+from crisp_py.robot import Robot
+from crisp_py.robot_config import RobotConfig
+import time
+
+
+config = RobotConfig(
+    joint_names=[
+        "panda_joint1",
+        "panda_joint2",
+        "panda_joint3",
+        "panda_joint4",
+        "panda_joint5",
+        "panda_joint6",
+        "panda_joint7",
+    ],
+    home_config=[0, -1.57, 1.57, 0, 1.57, 0, 0],
+    target_frame="panda_hand_tcp",
+    base_frame="panda_link0",
+    use_tf_pose=True,
+    tf_retrieve_rate=50.0,
+)
+
+robot = Robot(robot_config=config)
+
+robot.wait_until_ready()
+
+
+print(f"Current pose from TF: {robot.end_effector_pose}")
+print(f"Current pose after move: {robot.end_effector_pose}")
+
+