Ready topics in both nodes

stretch_core/nodes/stretch_driver

@@ -583,7 +583,7 @@ class StretchDriverNode:
             # odometry, and publish joint states
             while not rospy.is_shutdown():
                 self.robot.non_dxl_thread.step()
-                self.robot.pimu.set_fan_on()
+                # self.robot.pimu.set_fan_on()
                 self.command_mobile_base_velocity_and_publish_state()
                 self.robot.push_command()
                 self.dirty_command=False

stretch_demos/nodes/grasp_point

@@ -43,6 +43,7 @@ class GraspPointNode(hm.HelloNode):
     def disable_callback(self, msg):
         self.active = False
         self.update_perception = False
+        rospy.sleep(1)
         self.surface_mask = None
         self.grasp_targets = None
         self.best_target = None
@@ -111,6 +112,8 @@ class GraspPointNode(hm.HelloNode):
             return
         self.grasp_targets = grasp_targets
 
+        self.ready_pub.publish(Empty())
+
     def publish_markers(self):
         if self.surface_mask is not None:
             image_copy = self.manip.max_height_im.image.copy()
@@ -183,6 +186,7 @@ class GraspPointNode(hm.HelloNode):
         self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback)
         self.trigger_sub = rospy.Subscriber('/grasp_point/trigger_grasp_point', PointStamped, self.grasp_point_callback)
         self.enable_sub = rospy.Subscriber('/grasp_point/enable', Empty, self.enable_callback)
+        self.ready_pub = rospy.Publisher('/grasp_point/ready', Empty, queue_size=1)
         self.disable_sub = rospy.Subscriber('/grasp_point/disable', Empty, self.disable_callback)
         self.result_pub = rospy.Publisher('/grasp_point/result', Float32, queue_size=1)
 

stretch_demos/nodes/place_point

@@ -41,6 +41,7 @@ class PlacePointNode(hm.HelloNode):
     def disable_callback(self, msg):
         self.active = False
         self.update_perception = False
+        rospy.sleep(1)
         self.location_above_surface_m = None
         self.placement_target = None
         print(' - place_point disabled')
@@ -83,6 +84,8 @@ class PlacePointNode(hm.HelloNode):
             return
         self.placement_target = placement_target
 
+        self.ready_pub.publish(Empty())
+
     def publish_markers(self):
         if self.placement_target is not None:
             image_copy = self.manip.max_height_im.image.copy()
@@ -99,6 +102,7 @@ class PlacePointNode(hm.HelloNode):
         self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback)
         self.trigger_sub = rospy.Subscriber('/place_point/trigger_place_point', PointStamped, self.place_point_callback)
         self.enable_sub = rospy.Subscriber('/place_point/enable', Float32, self.enable_callback)
+        self.ready_pub = rospy.Publisher('/grasp_point/ready', Empty, queue_size=1)
         self.disable_sub = rospy.Subscriber('/place_point/disable', Empty, self.disable_callback)
         self.result_pub = rospy.Publisher('/place_point/result', Empty, queue_size=1)