You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
288 lines
10 KiB
288 lines
10 KiB
#!/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 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.08 #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 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')
|
|
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 = 45.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')
|
|
with self.joint_states_lock:
|
|
wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
|
|
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:
|
|
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)
|
|
rospy.sleep(1.0) # Give it time to finish the base move before the next move.
|
|
|
|
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 a 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 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()
|
|
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 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()
|
|
self.vertical_line(move_down=False)
|
|
|
|
def letter_o(self):
|
|
rospy.loginfo('Letter O')
|
|
# 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()
|
|
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)
|
|
|