code used for Hello World autonomy demo videos so far today

stretch_demos/launch/hello_world.launch

@@ -47,7 +47,7 @@
   <!-- -->
 
   <!-- KEYBOARD TELEOP -->
-  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--hello_world_on'/>
+  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on --hello_world_on'/>
   <!-- -->
     
 </launch>

stretch_demos/nodes/hello_world

@@ -37,7 +37,7 @@ class HelloWorldNode(hm.HelloNode):
         self.joint_states_lock = threading.Lock()
         self.move_base = nv.MoveBase(self)
         self.letter_height_m = 0.2
-        self.letter_top_lift_m = 1.05
+        self.letter_top_lift_m = 1.08 #1.05
         
     def joint_states_callback(self, joint_states):
         with self.joint_states_lock: 
@@ -48,29 +48,48 @@ class HelloWorldNode(hm.HelloNode):
         trigger_request = TriggerRequest() 
         trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+
+    def backoff_after_contact(self):
+        rospy.loginfo('backoff after contact')
+        with self.joint_states_lock:
+            wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
+        if wrist_position is not None:
+            wrist_target_m = wrist_position - 0.005
+            pose = {'wrist_extension': wrist_target_m}
+            self.move_to_pose(pose)
+            return True
+        else:
+            rospy.logerr('backoff_from_surface: self.wrist_position is None!')
+            return False
         
     def pen_down(self):
         rospy.loginfo('pen_down')
-        trigger_request = TriggerRequest() 
-        trigger_result = self.trigger_reach_until_contact_service(trigger_request)
-        rospy.loginfo('trigger_result = {0}'.format(trigger_result))
-
-    # def turn_off_contact_regulation(self):
-    #     rospy.loginfo('turn_off_contact_regulation')
-    #     trigger_request = TriggerRequest() 
-    #     trigger_result = self.trigger_turn_off_contact_regulation_service(trigger_request)
-    #     rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+        max_extension_m = 0.5
+        max_reach_m = 0.4
+        with self.joint_states_lock:
+            wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
+        extension_m = wrist_position + max_reach_m
+        extension_m = min(extension_m, max_extension_m)
+        extension_contact_effort = 42.0 #42.0 #40.0 from funmap
+        pose = {'wrist_extension': (extension_m, extension_contact_effort)}
+        self.move_to_pose(pose, custom_contact_thresholds=True)
         
     def pen_up(self):
         rospy.loginfo('pen_up')
-        #self.turn_off_contact_regulation()
         with self.joint_states_lock:
             wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
-        #backoff_m = 0.03 # back away 3cm from the surface
-        backoff_m = 0.1 # back away 10cm from the surface
-        new_wrist_position = wrist_position - backoff_m
-        pose = {'wrist_extension': new_wrist_position}
-        self.move_to_pose(pose)                
+        max_extension_m = 0.5
+        min_extension_m = 0.01
+        backoff_m = 0.1 #0.07 # back away 7cm from the surface
+        extension_m = wrist_position - backoff_m
+        extension_m = min(extension_m, max_extension_m)
+        extension_m = max(min_extension_m, extension_m)
+        extension_contact_effort = 80.0 #40.0 from funmap # to avoid stopping due to contact
+        pose = {'wrist_extension': (extension_m, extension_contact_effort)}
+        self.move_to_pose(pose, custom_contact_thresholds=True)
+        rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff.
+
         
     def vertical_line(self, move_down, half=False):
         if move_down: 
@@ -111,6 +130,7 @@ class HelloWorldNode(hm.HelloNode):
                 at_goal = self.move_base.forward(length_m, detect_obstacles=False)
             else:
                 at_goal = self.move_base.backward(length_m, detect_obstacles=False)
+        rospy.sleep(1.0) # Give it time to finish the base move before the next move.
 
     def space(self):
         rospy.loginfo('space')
@@ -137,7 +157,7 @@ class HelloWorldNode(hm.HelloNode):
 
     def letter_h(self):
         rospy.loginfo('Letter H')
-        #write an letter "H" down and to the right
+        #write a letter "H" down and to the right
         self.align_to_surface()
         self.pen_down()
         self.vertical_line(move_down=True)
@@ -154,10 +174,11 @@ class HelloWorldNode(hm.HelloNode):
 
     def letter_e(self):
         rospy.loginfo('Letter E')
-        # write an letter "E" down and to the right
+        # write a letter "E" down and to the right
         self.align_to_surface()
         self.pen_down()
         self.vertical_line(move_down=True)
+        self.pen_up()
         self.pen_down()
         self.horizontal_line(move_right=True)
         self.pen_up()
@@ -173,10 +194,11 @@ class HelloWorldNode(hm.HelloNode):
 
     def letter_l(self):
         rospy.loginfo('Letter L')
-        # write an letter "L" down and to the right
+        # write a letter "L" down and to the right
         self.align_to_surface()
         self.pen_down()
         self.vertical_line(move_down=True)
+        self.pen_up()
         self.pen_down()
         self.horizontal_line(move_right=True)
         self.pen_up()
@@ -184,14 +206,17 @@ class HelloWorldNode(hm.HelloNode):
 
     def letter_o(self):
         rospy.loginfo('Letter O')
-        # write an letter "O" down and to the right
+        # write a letter "O" down and to the right
         self.align_to_surface()
         self.pen_down()
         self.vertical_line(move_down=True)
+        self.pen_up()
         self.pen_down()
         self.horizontal_line(move_right=True)
+        self.pen_up()
         self.pen_down()
         self.vertical_line(move_down=False)
+        self.pen_up()
         self.pen_down()
         self.horizontal_line(move_right=False)
         self.pen_up()