Add TF docs to stretch_drivers

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)

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: