Create manipulation planning scene more often

stretch_demos/nodes/grasp_object

@@ -11,6 +11,7 @@ from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 
 from sensor_msgs.msg import PointCloud2
+from visualization_msgs.msg import Marker, MarkerArray
 
 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
 
@@ -51,12 +52,6 @@ class GraspObjectNode(hm.HelloNode):
         self.lift_position = lift_position
         self.left_finger_position, temp1, temp2 = hm.get_left_finger_state(joint_states)
 
-    def lower_tool_until_contact(self):
-        rospy.loginfo('lower_tool_until_contact')
-        trigger_request = TriggerRequest()
-        trigger_result = self.trigger_lower_until_contact_service(trigger_request)
-        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.get_tool())
         manip = self.manipulation_view
@@ -205,6 +200,14 @@ class GraspObjectNode(hm.HelloNode):
             message='Completed object grasp!'
         )
 
+    def temp_trigger_grasp_object_service2(self, request):
+        # grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer)
+
+        return TriggerResponse(
+            success=True,
+            message='Completed object grasp!'
+        )
+
     def main(self):
         hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False)
 
@@ -218,13 +221,9 @@ class GraspObjectNode(hm.HelloNode):
                                                            Trigger,
                                                            self.trigger_grasp_object_callback)
 
-        rospy.wait_for_service('/funmap/trigger_reach_until_contact')
-        rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
-        self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
-
-        rospy.wait_for_service('/funmap/trigger_lower_until_contact')
-        rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
-        self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
+        self.trigger_grasp_object_service2 = rospy.Service('/grasp_object/temp',
+                                                           Trigger,
+                                                           self.temp_trigger_grasp_object_service2)
 
         default_service = '/camera/switch_to_default_mode'
         high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
@@ -236,8 +235,18 @@ class GraspObjectNode(hm.HelloNode):
         rospy.loginfo('Node ' + self.node_name + ' connected to'  + high_accuracy_service)
         self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)
 
+        self.voi_marker_pub = rospy.Publisher(
+            '/grasp_object/voi_marker', Marker, queue_size=1)
+        self.mhi_marker_pub = rospy.Publisher(
+            '/grasp_object/mhi_marker', PointCloud2, queue_size=1)
+
+        self.manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool())
+        self.manip.move_head(self.move_to_pose, head_settle_time=0.75)
+
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
+            self.manip.update(self.point_cloud, self.tf2_buffer)
+            self.manip.publish_visualizations(self.voi_marker_pub, self.mhi_marker_pub)
             rate.sleep()
 
 

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -200,15 +200,14 @@ 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, tool='tool_stretch_gripper'):
         self.debug_directory = debug_directory
-        print('ManipulationView __init__: self.debug_directory =', self.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_aligned'
         }
         self.tool = tool
 
-        # Define the volume of interest for planning using the current
-        # view.
+        # Define the volume of interest for planning using the current view.
 
         # How far to look ahead.
         look_ahead_distance_m = 2.0
@@ -249,10 +248,10 @@ class ManipulationView():
         voi.change_frame(points_in_old_frame_to_new_frame_mat, new_frame_id)
         self.voi = voi
         self.max_height_im = rm.ROSMaxHeightImage(self.voi, m_per_pix, pixel_dtype)
-        self.max_height_im.print_info()
+        # self.max_height_im.print_info()
         self.updated = False
 
-    def move_head(self, move_to_pose):
+    def move_head(self, move_to_pose, head_settle_time=0.5):
         tilt = -0.8
         pan = -1.8 #-1.6
         # This head configuration can reduce seeing the hand or arm when they are held high, which can avoid noise due to the hand and arm being to close to the head.
@@ -260,7 +259,6 @@ class ManipulationView():
         #pan = -0.9
         pose = {'joint_head_pan': pan, 'joint_head_tilt': tilt}
         move_to_pose(pose)
-        head_settle_time = 0.5
         rospy.sleep(head_settle_time)
 
     def estimate_reach_to_contact_distance(self, tooltip_frame, tf2_buffer, save_debugging_images=True):

stretch_funmap/src/stretch_funmap/max_height_image.py

@@ -94,6 +94,9 @@ class VolumeOfInterest:
         origin.append(1.0)
         origin = np.array(origin)
         new_origin = np.matmul(points_in_old_frame_to_new_frame_mat, origin)
+        new_origin = list(new_origin)
+        new_origin.pop()
+        new_origin = np.array(new_origin)
         # rotate the axes
         new_axes = np.matmul(points_in_old_frame_to_new_frame_mat[:3,:3], self.axes)
         # transform transforms
@@ -160,6 +163,7 @@ class MaxHeightImage:
         # pixel_dtype : Numpy dtype for the max height image (MHI)
         # pixels, which also defines the maximum resolution in height.
         #
+        self.last_update_time = None
         self.supported_dtypes = [np.uint8, np.uint16, np.uint32, np.uint64, np.float32, np.float64]
 
         if pixel_dtype not in self.supported_dtypes:

stretch_funmap/src/stretch_funmap/ros_max_height_image.py

@@ -73,7 +73,7 @@ class ROSVolumeOfInterest(VolumeOfInterest):
         marker.type = marker.CUBE
         marker.action = marker.ADD
         marker.lifetime = rospy.Duration(duration)
-        marker.text = 'volume of interest'
+        # marker.text = 'volume of interest'
 
         marker.header.frame_id = self.frame_id
         marker.header.stamp = rospy.Time.now()
@@ -276,5 +276,7 @@ class ROSMaxHeightImage(MaxHeightImage):
             
     def to_point_cloud(self, color_map=None):
         points = self.to_points(color_map)
+        if self.last_update_time is None:
+            self.last_update_time = rospy.Time.now()
         point_cloud = ros_numpy.msgify(PointCloud2, points, stamp=self.last_update_time, frame_id=self.voi.frame_id)
         return point_cloud