From 5af1f67e86cb9d81131e54741ffe731df14607d1 Mon Sep 17 00:00:00 2001
From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com>
Date: Tue, 30 Jun 2020 22:36:13 -0400
Subject: [PATCH] added old demos for whiteboard writing & drawer pulling
---
stretch_demos/launch/hello_world.launch | 53 +++++
stretch_demos/launch/open_drawer.launch | 51 +++++
stretch_demos/nodes/hello_world | 266 ++++++++++++++++++++++++
stretch_demos/nodes/open_drawer | 179 ++++++++++++++++
4 files changed, 549 insertions(+)
create mode 100644 stretch_demos/launch/hello_world.launch
create mode 100644 stretch_demos/launch/open_drawer.launch
create mode 100755 stretch_demos/nodes/hello_world
create mode 100755 stretch_demos/nodes/open_drawer
diff --git a/stretch_demos/launch/hello_world.launch b/stretch_demos/launch/hello_world.launch
new file mode 100644
index 0000000..64d6dd6
--- /dev/null
+++ b/stretch_demos/launch/hello_world.launch
@@ -0,0 +1,53 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stretch_demos/launch/open_drawer.launch b/stretch_demos/launch/open_drawer.launch
new file mode 100644
index 0000000..90639e5
--- /dev/null
+++ b/stretch_demos/launch/open_drawer.launch
@@ -0,0 +1,51 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world
new file mode 100755
index 0000000..a229f9e
--- /dev/null
+++ b/stretch_demos/nodes/hello_world
@@ -0,0 +1,266 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+import rospy
+import actionlib
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryGoal
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+from sensor_msgs.msg import PointCloud2
+
+from control_msgs.msg import GripperCommandAction
+from control_msgs.msg import GripperCommandGoal
+
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+
+import math
+import time
+import threading
+import sys
+import tf2_ros
+import argparse as ap
+
+import hello_helpers.hello_misc as hm
+import stretch_funmap.navigate as nv
+
+
+
+class HelloWorldNode(hm.HelloNode):
+
+ def __init__(self):
+ hm.HelloNode.__init__(self)
+ self.rate = 10.0
+ self.joint_states = None
+ 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
+
+ def joint_states_callback(self, joint_states):
+ with self.joint_states_lock:
+ self.joint_states = joint_states
+
+ def align_to_surface(self):
+ rospy.loginfo('align_to_surface')
+ trigger_request = TriggerRequest()
+ trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
+ rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+ 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))
+
+ 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)
+
+ def vertical_line(self, move_down, half=False):
+ if move_down:
+ rospy.loginfo('vertical_line moving down')
+ else:
+ rospy.loginfo('vertical_line moving up')
+ with self.joint_states_lock:
+ i = self.joint_states.name.index('joint_lift')
+ lift_position = self.joint_states.position[i]
+ if not half:
+ length_m = self.letter_height_m
+ else:
+ length_m = self.letter_height_m / 2.0
+
+ if move_down:
+ new_lift_position = lift_position - length_m
+ else:
+ new_lift_position = lift_position + length_m
+ pose = {'joint_lift': new_lift_position}
+ self.move_to_pose(pose)
+
+ def horizontal_line(self, move_right, half=False):
+ if move_right:
+ rospy.loginfo('horizontal_line moving right')
+ else:
+ rospy.loginfo('horizontal_line moving left')
+ length_m = self.letter_height_m / 2.0
+ if half:
+ length_m = length_m / 2.0
+ if move_right:
+ length_m = -length_m
+ use_simple_move = False
+ if use_simple_move:
+ pose = {'translate_mobile_base': length_m}
+ self.move_to_pose(pose)
+ else:
+ if length_m > 0:
+ at_goal = self.move_base.forward(length_m, detect_obstacles=False)
+ else:
+ at_goal = self.move_base.backward(length_m, detect_obstacles=False)
+
+ def space(self):
+ rospy.loginfo('space')
+ self.align_to_surface()
+ self.horizontal_line(move_right=True, half=True)
+
+
+ def move_arm_out_of_the_way(self):
+ initial_pose = {'wrist_extension': 0.01,
+ 'joint_lift': 0.8,
+ 'joint_wrist_yaw': 0.0}
+
+ rospy.loginfo('Move arm out of the way of the camera.')
+ self.move_to_pose(initial_pose)
+
+
+ def move_to_initial_configuration(self):
+ initial_pose = {'wrist_extension': 0.01,
+ 'joint_lift': self.letter_top_lift_m,
+ 'joint_wrist_yaw': 0.0}
+
+ rospy.loginfo('Move to initial arm pose for writing.')
+ self.move_to_pose(initial_pose)
+
+ def letter_h(self):
+ rospy.loginfo('Letter H')
+ #write an letter "H" down and to the right
+ self.align_to_surface()
+ self.pen_down()
+ self.vertical_line(move_down=True)
+ self.pen_up()
+ self.vertical_line(move_down=False, half=True)
+ self.pen_down()
+ self.horizontal_line(move_right=True)
+ self.pen_up()
+ self.vertical_line(move_down=False, half=True)
+ self.pen_down()
+ self.vertical_line(move_down=True)
+ self.pen_up()
+ self.vertical_line(move_down=False)
+
+ def letter_e(self):
+ rospy.loginfo('Letter E')
+ # write an letter "E" down and to the right
+ self.align_to_surface()
+ self.pen_down()
+ self.vertical_line(move_down=True)
+ self.pen_down()
+ self.horizontal_line(move_right=True)
+ self.pen_up()
+ self.vertical_line(move_down=False)
+ self.pen_down()
+ self.horizontal_line(move_right=False)
+ self.pen_up()
+ self.vertical_line(move_down=True, half=True)
+ self.pen_down()
+ self.horizontal_line(move_right=True)
+ self.pen_up()
+ self.vertical_line(move_down=False, half=True)
+
+ def letter_l(self):
+ rospy.loginfo('Letter L')
+ # write an letter "L" down and to the right
+ self.align_to_surface()
+ self.pen_down()
+ self.vertical_line(move_down=True)
+ self.pen_down()
+ self.horizontal_line(move_right=True)
+ self.pen_up()
+ self.vertical_line(move_down=False)
+
+ def letter_o(self):
+ rospy.loginfo('Letter O')
+ # write an letter "O" down and to the right
+ self.align_to_surface()
+ self.pen_down()
+ self.vertical_line(move_down=True)
+ self.pen_down()
+ self.horizontal_line(move_right=True)
+ self.pen_down()
+ self.vertical_line(move_down=False)
+ self.pen_down()
+ self.horizontal_line(move_right=False)
+ self.pen_up()
+ self.horizontal_line(move_right=True)
+
+ def trigger_write_hello_callback(self, request):
+
+ #self.move_arm_out_of_the_way()
+
+ self.move_to_initial_configuration()
+
+ self.align_to_surface()
+
+ self.letter_h()
+ self.space()
+ self.letter_e()
+ self.space()
+ self.letter_l()
+ self.space()
+ self.letter_l()
+ self.space()
+ self.letter_o()
+ self.space()
+
+ return TriggerResponse(
+ success=True,
+ message='Completed writing hello!'
+ )
+
+
+ def main(self):
+ hm.HelloNode.main(self, 'hello_world', 'hello_world', wait_for_first_pointcloud=False)
+
+ self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
+
+ self.trigger_write_hello_service = rospy.Service('/hello_world/trigger_write_hello',
+ Trigger,
+ self.trigger_write_hello_callback)
+
+ rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
+ rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
+ self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
+
+ 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_turn_off_contact_regulation')
+ # rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_turn_off_contact_regulation.')
+ # self.trigger_turn_off_contact_regulation_service = rospy.ServiceProxy('/funmap/trigger_turn_off_contact_regulation', Trigger)
+
+ rate = rospy.Rate(self.rate)
+ while not rospy.is_shutdown():
+ rate.sleep()
+
+
+if __name__ == '__main__':
+ try:
+ parser = ap.ArgumentParser(description='Hello World demo for stretch.')
+ #parser.add_argument('--mapping_on', action='store_true', help='Turn on mapping control. For example, the space bar will trigger a head scan. This requires that the mapping node be run (funmap).')
+ args, unknown = parser.parse_known_args()
+ node = HelloWorldNode()
+ node.main()
+ except KeyboardInterrupt:
+ rospy.loginfo('interrupt received, so shutting down')
+# except rospy.ROSInterruptException:
+# pass
+ #rospy.loginfo('keyboard_teleop was interrupted', file=sys.stderr)
+
diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer
new file mode 100755
index 0000000..1c12d97
--- /dev/null
+++ b/stretch_demos/nodes/open_drawer
@@ -0,0 +1,179 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+import rospy
+import actionlib
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryGoal
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+from sensor_msgs.msg import PointCloud2
+
+from control_msgs.msg import GripperCommandAction
+from control_msgs.msg import GripperCommandGoal
+
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+
+import math
+import time
+import threading
+import sys
+
+import tf2_ros
+import argparse as ap
+
+import hello_helpers.hello_misc as hm
+import stretch_funmap.navigate as nv
+
+class OpenDrawerNode(hm.HelloNode):
+
+ def __init__(self):
+ hm.HelloNode.__init__(self)
+ self.rate = 10.0
+ self.joint_states = None
+ self.joint_states_lock = threading.Lock()
+ self.move_base = nv.MoveBase(self)
+ self.letter_height_m = 0.2
+ self.wrist_position = None
+
+ def joint_states_callback(self, joint_states):
+ with self.joint_states_lock:
+ self.joint_states = joint_states
+ wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
+ self.wrist_position = wrist_position
+
+ def align_to_surface(self):
+ rospy.loginfo('align_to_surface')
+ trigger_request = TriggerRequest()
+ trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
+ rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+ def extend_hook_until_contact(self):
+ rospy.loginfo('extend_hook_until_contact')
+ trigger_request = TriggerRequest()
+ trigger_result = self.trigger_reach_until_contact_service(trigger_request)
+ rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+ def lower_hook_until_contact(self):
+ rospy.loginfo('lower_hook_until_contact')
+ trigger_request = TriggerRequest()
+ trigger_result = self.trigger_lower_until_contact_service(trigger_request)
+ rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
+ def backoff_from_surface(self):
+ rospy.loginfo('backoff_from_surface')
+ if self.wrist_position is not None:
+ wrist_target_m = self.wrist_position - 0.012 #0.015 #0.018 #0.02 #0.022 #0.025 #0.008 #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 pull_open(self):
+ rospy.loginfo('pull_open')
+ if self.wrist_position is not None:
+ wrist_target_m = self.wrist_position - 0.2
+ pose = {'wrist_extension': wrist_target_m}
+ self.move_to_pose(pose)
+ return True
+ else:
+ rospy.logerr('pull_open: self.wrist_position is None!')
+ return False
+
+ def push_closed(self):
+ rospy.loginfo('push_closed')
+ if self.wrist_position is not None:
+ wrist_target_m = self.wrist_position + 0.22
+ pose = {'wrist_extension': wrist_target_m}
+ self.move_to_pose(pose)
+ return True
+ else:
+ rospy.logerr('pull_open: self.wrist_position is None!')
+ return False
+
+ def move_to_initial_configuration(self):
+ initial_pose = {'wrist_extension': 0.01,
+ 'joint_gripper': 1.62,
+ 'joint_gripper_finger_left': -0.25}
+ #'joint_lift': 0.71,
+ rospy.loginfo('Move to the initial configuration for drawer opening.')
+ self.move_to_pose(initial_pose)
+
+ def trigger_open_drawer_callback(self, request):
+
+ self.move_to_initial_configuration()
+
+ #self.align_to_surface()
+ self.extend_hook_until_contact()
+ success = self.backoff_from_surface()
+ if not success:
+ return TriggerResponse(
+ success=False,
+ message='Failed to backoff from the surface.'
+ )
+
+ self.lower_hook_until_contact()
+ success = self.pull_open()
+ if not success:
+ return TriggerResponse(
+ success=False,
+ message='Failed to pull open the drawer.'
+ )
+
+ push_drawer_closed = False
+ if push_drawer_closed:
+ rospy.sleep(3.0)
+ self.push_closed()
+
+ return TriggerResponse(
+ success=True,
+ message='Completed opening the drawer!'
+ )
+
+
+ def main(self):
+ hm.HelloNode.main(self, 'open_drawer', 'open_drawer', wait_for_first_pointcloud=False)
+
+ self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
+
+ self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer',
+ Trigger,
+ self.trigger_open_drawer_callback)
+
+ rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
+ rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
+ self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
+
+ 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)
+
+ rate = rospy.Rate(self.rate)
+ while not rospy.is_shutdown():
+ rate.sleep()
+
+
+if __name__ == '__main__':
+ try:
+ parser = ap.ArgumentParser(description='Open Drawer behavior for stretch.')
+ #parser.add_argument('--mapping_on', action='store_true', help='Turn on mapping control. For example, the space bar will trigger a head scan. This requires that the mapping node be run (funmap).')
+ args, unknown = parser.parse_known_args()
+ node = OpenDrawerNode()
+ node.main()
+ except KeyboardInterrupt:
+ rospy.loginfo('interrupt received, so shutting down')
+# except rospy.ROSInterruptException:
+# pass
+ #rospy.loginfo('keyboard_teleop was interrupted', file=sys.stderr)
+