From a37c01cfe88b430b61cb3d447fb477453c79b8d9 Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Sat, 4 Jul 2020 13:36:04 -0400 Subject: [PATCH] code used for Hello World autonomy demo videos so far today --- stretch_demos/launch/hello_world.launch | 2 +- stretch_demos/nodes/hello_world | 65 +++++++++++++++++-------- 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/stretch_demos/launch/hello_world.launch b/stretch_demos/launch/hello_world.launch index 64d6dd6..b2e8e9f 100644 --- a/stretch_demos/launch/hello_world.launch +++ b/stretch_demos/launch/hello_world.launch @@ -47,7 +47,7 @@ - + diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world index 6a68f7f..7b6ee9f 100755 --- a/stretch_demos/nodes/hello_world +++ b/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()