From 45952ce87969f47ce506f35f48cd83da18b60d53 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Thu, 28 Sep 2023 23:25:32 -0700 Subject: [PATCH 1/3] Add HelloNode.get_joint_state() + other improvements The get_joint_state() method makes it easier to get information about a joint, including whether the joint is moving. A mode variable is added to make it easier to determine what mode the driver is in. Lastly, some of HelloNode's variables are made private to make it easier to use the class from iPython. --- hello_helpers/src/hello_helpers/hello_misc.py | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 459f7a8..acbf645 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/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,18 @@ 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): + 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 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 +227,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') From b3ef7684cd3796bcfc04ab4963e23e9c1f3080f2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 29 Sep 2023 00:11:03 -0700 Subject: [PATCH 2/3] Add HelloNode.get_point_cloud() --- hello_helpers/src/hello_helpers/hello_misc.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index acbf645..5f8189d 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -210,6 +210,7 @@ class HelloNode: 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] @@ -217,6 +218,14 @@ class HelloNode: 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() From 56fc1bb1b2f8b6cdcddf9bdb08808bf21cf3db38 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 29 Sep 2023 00:48:57 -0700 Subject: [PATCH 3/3] Add docs for new HelloNode methods --- hello_helpers/README.md | 54 ++++++++++++++++++++++++++++++++++++++--- 1 file changed, 51 insertions(+), 3 deletions(-) diff --git a/hello_helpers/README.md b/hello_helpers/README.md index 9ef056e..8ee1851 100644 --- a/hello_helpers/README.md +++ b/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.