diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 151ceed..95d493d 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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 diff --git a/stretch_demos/nodes/grasp_point b/stretch_demos/nodes/grasp_point index 979a3ac..cdd16fa 100755 --- a/stretch_demos/nodes/grasp_point +++ b/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) diff --git a/stretch_demos/nodes/place_point b/stretch_demos/nodes/place_point index 472fc23..674ed8a 100755 --- a/stretch_demos/nodes/place_point +++ b/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)