From ff83bafc0e185ba69516bc1f45490941c44fc948 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 21 Feb 2023 12:52:23 -0800 Subject: [PATCH] Add docs for HelloNode.get_tf --- hello_helpers/README.md | 36 ++++++++----------- hello_helpers/src/hello_helpers/hello_misc.py | 2 +- stretch_core/README.md | 4 +-- 3 files changed, 17 insertions(+), 25 deletions(-) diff --git a/hello_helpers/README.md b/hello_helpers/README.md index c403cc2..b567311 100644 --- a/hello_helpers/README.md +++ b/hello_helpers/README.md @@ -81,6 +81,20 @@ If you set `custom_full_goal` to True, the dictionary format is string/tuple key self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True) ``` +##### `get_tf(from_frame, to_frame)` + +Use this method to get the transform ([geometry_msgs/TransformStamped](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html)) between two frames. This method is blocking. For example, this method can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using: + +```python +# roslaunch the stretch launch file beforehand + +import rospy +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +t = temp.get_tf('base_link', 'link_grasp_center') +print(t.transform.translation) +``` + ##### `get_robot_floor_pose_xya(floor_frame='odom')` Returns the current estimated x, y position and angle of the robot on the floor. This is typically called with respect to the odom frame or the map frame. x and y are in meters and the angle is in radians. @@ -132,28 +146,6 @@ temp = hm.HelloNode.quick_create('temp') temp.stop_the_robot_service(TriggerRequest()) ``` -#### Other - -##### TF Listener - -Provides a [tf2](http://wiki.ros.org/tf2) listener that buffers [transforms](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html) under the `self.tf2_buffer` attribute. For example, we can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using: - -```python -# roslaunch the stretch launch file beforehand - -import rospy -import hello_helpers.hello_misc as hm -temp = hm.HelloNode.quick_create('temp') -rate = rospy.Rate(10.0) -while True: - try: - t = temp.tf2_buffer.lookup_transform('link_grasp_center', 'base_link', rospy.Time()) - print(t.transform.translation, t.transform.rotation) - except: - continue - rate.sleep() -``` - ## License For license information, please see the LICENSE files. diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index c9203a5..50f79c8 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -159,7 +159,7 @@ class HelloNode: rate = rospy.Rate(10.0) while True: try: - return self.tf2_buffer.lookup_transform(to_frame, from_frame, rospy.Time()) + return self.tf2_buffer.lookup_transform(from_frame, to_frame, rospy.Time()) except: continue rate.sleep() diff --git a/stretch_core/README.md b/stretch_core/README.md index 0b39109..8363a13 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -50,7 +50,7 @@ Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool See [stretch_driver.launch's TF docs](#TODO) to learn about the full Stretch TF tree. The stretch_driver node, which is part of stretch_driver.launch, is responsible for publishing the "odom" to "base_link" transform if the [broadcast_odom_tf parameter](#broadcastodomtf) is set to true. Odometry for the robot is calculated within the underlying Stretch Body Python SDK within the [update method in the Base class](https://github.com/hello-robot/stretch_body/blob/ea987c3d4f21a65ce4e85c6c92cd5d2efb832c41/body/stretch_body/base.py#L555-L651) by looking at the encoders for the left and right wheels. Note: Odometry calculated from wheel encoders is susceptible to drift for a variety of reasons (e.g. wheel slip, misalignment, loose belt tension, time dilation). A reasonable amount of drift is ~1cm per meter translated by the mobile base. A common remedy is to use a localization library to correct for the drift by integrating other sensors into the calculation of the odometry. For example, [AMCL](http://wiki.ros.org/amcl) is a particle filter ROS package for using Lidar scans + wheel odometry to correct for drift. -If you use [HelloNode](../hello_helpers/README.md#hellonodesrchellohelpershellomiscpy), you can get the odometry using: +If you use [HelloNode](../hello_helpers/README.md#hellonode), you can get the odometry using: ```python # roslaunch the stretch launch file beforehand @@ -58,7 +58,7 @@ If you use [HelloNode](../hello_helpers/README.md#hellonodesrchellohelpershellom import rospy import hello_helpers.hello_misc as hm temp = hm.HelloNode.quick_create('temp') -t = temp.get_tf('base_link', 'odom') +t = temp.get_tf('odom', 'base_link') print(t.transform.translation) ```