Add stretch_driver topic for end_of_arm tool

hello_helpers/src/hello_helpers/hello_misc.py

@@ -20,6 +20,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint
 import tf2_ros
 from sensor_msgs.msg import PointCloud2
 from std_srvs.srv import Trigger, TriggerRequest
+from std_msgs.msg import String
 
 
 #######################
@@ -69,6 +70,7 @@ class HelloNode:
     def __init__(self):
         self.joint_state = None
         self.point_cloud = None
+        self.tool = None
 
     @classmethod
     def quick_create(cls, name, wait_for_first_pointcloud=False):
@@ -81,7 +83,10 @@ class HelloNode:
 
     def point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
-    
+
+    def tool_callback(self, tool_string):
+        self.tool = tool_string.data
+
     def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
         point = JointTrajectoryPoint()
         point.time_from_start = rospy.Duration(0.0)
@@ -182,6 +187,10 @@ class HelloNode:
         rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}")
         return trigger_result.success
 
+    def get_tool(self):
+        assert(self.tool is not None)
+        return self.tool
+
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
@@ -195,7 +204,9 @@ class HelloNode:
         
         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.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)
 

stretch_core/nodes/stretch_driver

@@ -197,6 +197,11 @@ class StretchDriverNode:
         mode_msg.data = self.robot_mode
         self.mode_pub.publish(mode_msg)
 
+        # publish end of arm tool
+        tool_msg = String()
+        tool_msg.data = self.robot.end_of_arm.name
+        self.tool_pub.publish(tool_msg)
+
         ##################################################
         # publish joint state
         joint_state = JointState()
@@ -508,6 +513,7 @@ class StretchDriverNode:
         self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1)
         self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1)
         self.mode_pub = rospy.Publisher('mode', String, queue_size=1)
+        self.tool_pub = rospy.Publisher('tool', String, queue_size=1)
 
         self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1)
         self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1)

stretch_demos/nodes/grasp_object

@@ -58,7 +58,7 @@ class GraspObjectNode(hm.HelloNode):
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
     def look_at_surface(self, scan_time_s=None):
-        self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
+        self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool())
         manip = self.manipulation_view
         head_settle_time_s = 0.02 #1.0
         manip.move_head(self.move_to_pose)

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -198,10 +198,15 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
     
 
 class ManipulationView():
-    def __init__(self, tf2_buffer, debug_directory=None):
+    def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'):
         self.debug_directory = debug_directory
         print('ManipulationView __init__: self.debug_directory =', self.debug_directory)
-        
+        self.gripper_links = {
+            'tool_stretch_gripper': 'link_gripper',
+            'tool_stretch_dex_wrist': 'link_straight_gripper'
+        }
+        self.tool = tool
+
         # Define the volume of interest for planning using the current
         # view.
         
@@ -420,7 +425,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
         #
         # forward_direction = np.array([1.0, 0.0, 0.0])
@@ -469,7 +474,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
 
         # Obtain the gripper yaw axis location in the image by
@@ -582,7 +587,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
 
         # Obtain the gripper yaw axis location in the image by