Merge pull request #115 from hello-robot/feature/hellonode_jointstates

Add HelloNode.get_joint_state() + other improvements to HelloNode

hello_helpers/README.md

@@ -8,7 +8,7 @@
 
 *fit_plane.py* : Fits planes to 3D data.
 
-*hello_misc.py* : Various functions, including a helpful Python object with which to create ROS nodes. 
+*hello_misc.py* : Various functions, including a helpful Python object with which to create ROS nodes.
 
 *hello_ros_viz.py* : Various helper functions for vizualizations using RViz.
 
@@ -59,6 +59,23 @@ temp.move_to_pose({'joint_lift': 0.4})
 
 #### Attributes
 
+##### `joint_states`
+
+This attribute gives you the entire joint state of the robot as a [JointState](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html) message. The utility method [`get_joint_state()`](#get_joint_statejoint_name-moving_threshold0001) is an easier alternative to parsing the JointState message.
+
+##### `point_cloud`
+
+This attribute is a [PointCloud2](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html) message as seen by the head camera. The utility method [`get_point_cloud()`](#get_point_cloud) is an easier alternative to parsing the PointCloud2 message.
+
+##### `tool`
+
+This attribute is the name of the end-effector as a string. You can use this attribute to flag an error for other Stretch users if their robot isn't configured with the correct tool. Most commonly, this attribute will be either `'tool_stretch_dex_wrist'` or `'tool_stretch_gripper'`. To learn about the other end-effectors available for Stretch, or how to create your own and plug it into Stretch's ROS driver, check out the [documentation on tools](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_tool_change/).
+
+##### `mode`
+
+This attribute is the mode the robot's driver is in, as a string. See the driver's API to learn more about [driver modes](../stretch_core/README.md#mode-std_msgsstring).
+
+
 ##### `dryrun`
 
 This attribute allows you to control whether the robot actually moves when calling `move_to_pose()`, `home_the_robot()`, `stow_the_robot()`, or other motion methods in this class. When `dryrun` is set to True, these motion methods return immediately. This attribute is helpful when you want to run just the perception/planning part of your node without actually moving the robot. For example, you could replace the following verbose snippet:
@@ -127,13 +144,44 @@ Use this method to get the transform ([geometry_msgs/TransformStamped](https://d
 ```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_joint_state(joint_name, moving_threshold=0.001)`
+
+Use this method to retrieve the joint state for a single joint. It will return a tuple with joint position, velocity, effort, and is_moving as a boolean (checked against the moving_threshold argument). For example:
+
+```python
+# roslaunch the stretch launch file beforehand
+
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+pos, vel, eff, is_moving = temp.get_joint_state('joint_head_pan')
+print(f"The head pan is {'' if is_moving else 'not'} moving")
+```
+
+##### `get_point_cloud()`
+
+Use this method to retrieve the point cloud seen by the head camera as a Numpy array. It will return a tuple with a named array, Nx3 3D point array, timestamp at which the point was captured, and TF frame in which the cloud was captured. For example:
+
+```python
+# roslaunch the stretch launch file beforehand
+
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+cloud, cloud_xyz, capture_time, capture_frame = temp.get_point_cloud()
+print(f"Head camera saw a cloud of size {cloud_xyz.shape} in frame {capture_frame} at {capture_time}")
+# Head camera saw a cloud of size (275925, 3) in frame camera_color_optical_frame at 1695973195045439959
+
+import numpy as np
+i = np.argmax(cloud['z'])
+print(f"If the capture frame is camera_color_optical_frame (i.e. z axis points out from camera), the point furthest from the camera is {np.sqrt(cloud['x'][i]**2 + cloud['y'][i]**2 + cloud['z'][i]**2):.2f}m away and has the color {(cloud['r'][i], cloud['g'][i], cloud['b'][i])}")
+# If the capture frame is camera_color_optical_frame (i.e. z axis points out from camera), the point furthest from the camera is 1.81m away and has the color (118, 121, 103)
+```
+
 ##### `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.
@@ -187,4 +235,4 @@ temp.stop_the_robot_service(TriggerRequest())
 
 ## License
 
-For license information, please see the LICENSE files. 
+For license information, please see the LICENSE files.

hello_helpers/src/hello_helpers/hello_misc.py

@@ -18,7 +18,7 @@ from control_msgs.msg import FollowJointTrajectoryAction
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import tf2_ros
-from sensor_msgs.msg import PointCloud2
+from sensor_msgs.msg import PointCloud2, JointState
 from std_srvs.srv import Trigger, TriggerRequest
 from std_msgs.msg import String
 
@@ -68,9 +68,10 @@ def get_left_finger_state(joint_states):
 
 class HelloNode:
     def __init__(self):
-        self.joint_state = None
+        self.joint_states = None
         self.point_cloud = None
         self.tool = None
+        self.mode = None
         self.dryrun = False
 
     @classmethod
@@ -79,15 +80,18 @@ class HelloNode:
         i.main(name, name, wait_for_first_pointcloud)
         return i
 
-    def joint_states_callback(self, joint_state):
-        self.joint_state = joint_state
+    def _joint_states_callback(self, joint_states):
+        self.joint_states = joint_states
 
-    def point_cloud_callback(self, point_cloud):
+    def _point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
 
-    def tool_callback(self, tool_string):
+    def _tool_callback(self, tool_string):
         self.tool = tool_string.data
 
+    def _mode_callback(self, mode_string):
+        self.mode = mode_string.data
+
     def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
         if self.dryrun:
             return
@@ -201,6 +205,27 @@ class HelloNode:
         assert(self.tool is not None)
         return self.tool
 
+    def get_mode(self):
+        assert(self.mode is not None)
+        return self.mode
+
+    def get_joint_state(self, joint_name, moving_threshold=0.001):
+        assert(self.joint_states is not None)
+        i = self.joint_states.name.index(joint_name)
+        joint_position = self.joint_states.position[i]
+        joint_velocity = self.joint_states.velocity[i]
+        joint_effort = self.joint_states.effort[i]
+        joint_is_moving = abs(joint_velocity) > moving_threshold
+        return (joint_position, joint_velocity, joint_effort, joint_is_moving)
+
+    def get_point_cloud(self):
+        assert(self.point_cloud is not None)
+        cloud = ros_numpy.point_cloud2.split_rgb_field(ros_numpy.numpify(self.point_cloud))
+        cloud_xyz = ros_numpy.point_cloud2.get_xyz_points(cloud)
+        cloud_time = self.point_cloud.header.stamp
+        cloud_frame = self.point_cloud.header.frame_id
+        return (cloud, cloud_xyz, cloud_time, cloud_frame)
+
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
@@ -211,13 +236,16 @@ class HelloNode:
         if not server_reached:
             rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.')
             sys.exit()
-        
+
+        self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback)
+
         self.tf2_buffer = tf2_ros.Buffer()
         self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
 
-        self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback)
+        self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback)
+        self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback)
 
-        self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
+        self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback)
         self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
 
         rospy.wait_for_service('/home_the_robot')