From bc90962c33629bd808bc9e8bf8d4ac0c7becf61d Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 21 Feb 2023 12:20:38 -0800 Subject: [PATCH] Add TF docs to stretch_drivers --- hello_helpers/src/hello_helpers/hello_misc.py | 10 +++++++++ stretch_core/README.md | 22 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index f9e5798..c9203a5 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -153,6 +153,16 @@ class HelloNode: return [r0[0], r0[1], r_ang], timestamp + def get_tf(self, from_frame, to_frame): + """Get current transform between 2 frames. Blocking. + """ + rate = rospy.Rate(10.0) + while True: + try: + return self.tf2_buffer.lookup_transform(to_frame, from_frame, rospy.Time()) + except: + continue + rate.sleep() def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): rospy.init_node(node_name) diff --git a/stretch_core/README.md b/stretch_core/README.md index 9f14c6d..0b39109 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -18,6 +18,12 @@ ### [stretch_driver](./nodes/stretch_driver) +#### Parameters + +##### broadcast_odom_tf + +If set to true, stretch_driver will publish an odom to base_link TF. + #### Published Topics ##### /battery ([sensor_msgs/BatteryState](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/BatteryState.html)) @@ -40,6 +46,22 @@ This service will start Stretch's stowing procedure, where the arm is stowed int Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool from a terminal, or calling [`robot.stow()`](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_stretch_body_api/#stretch_body.robot.Robot.stow) from Stretch's Python API. +#### Transforms + +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: + +```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', 'odom') +print(t.transform.translation) +``` + # Testing The primary testing framework being used within *stretch_ros* is pytest. Pytest is an open source testing framework that scales well and takes a functional approach resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date: