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. diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 459f7a8..5f8189d 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,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')