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.
250 lines
11 KiB
250 lines
11 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 os
|
|
import tf2_ros
|
|
import argparse as ap
|
|
|
|
import hello_helpers.hello_misc as hm
|
|
import stretch_funmap.navigate as nv
|
|
import stretch_funmap.manipulation_planning as mp
|
|
|
|
|
|
class CleanSurfaceNode(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
|
|
self.lift_position = None
|
|
self.manipulation_view = None
|
|
self.debug_directory = 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
|
|
lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
|
|
self.lift_position = lift_position
|
|
|
|
def lower_tool_until_contact(self):
|
|
rospy.loginfo('lower_tool_until_contact')
|
|
trigger_request = TriggerRequest()
|
|
trigger_result = self.trigger_lower_until_contact_service(trigger_request)
|
|
rospy.loginfo('trigger_result = {0}'.format(trigger_result))
|
|
|
|
def wipe_in(self):
|
|
rospy.loginfo('wipe_in')
|
|
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('wipe_in: self.wrist_position is None!')
|
|
return False
|
|
|
|
def wipe_out(self):
|
|
rospy.loginfo('wipe_out')
|
|
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('wipe_out: self.wrist_position is None!')
|
|
return False
|
|
|
|
def look_at_surface(self):
|
|
self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
|
|
manip = self.manipulation_view
|
|
manip.move_head(self.move_to_pose)
|
|
manip.update(self.point_cloud, self.tf2_buffer)
|
|
if self.debug_directory is not None:
|
|
dirname = self.debug_directory + 'clean_surface/'
|
|
# If the directory does not already exist, create it.
|
|
if not os.path.exists(dirname):
|
|
os.makedirs(dirname)
|
|
filename = 'look_at_surface_' + hm.create_time_string()
|
|
manip.save_scan(dirname + filename)
|
|
else:
|
|
rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.')
|
|
|
|
def trigger_clean_surface_callback(self, request):
|
|
|
|
tool_width_m = 0.08 #0.06
|
|
tool_length_m = 0.08 #0.06
|
|
step_size_m = 0.04 #0.06 #0.1 #0.02
|
|
min_extension_m = 0.01
|
|
max_extension_m = 0.5
|
|
|
|
self.look_at_surface()
|
|
strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m, step_size_m)
|
|
|
|
print('********* lift_to_surface_m = {0} **************'.format(lift_to_surface_m))
|
|
|
|
if True and (strokes is not None) and (len(strokes) > 0):
|
|
|
|
if (self.lift_position is not None) and (self.wrist_position is not None):
|
|
|
|
above_surface_m = 0.1
|
|
|
|
lift_above_surface_m = self.lift_position + lift_to_surface_m + above_surface_m
|
|
pose = {'joint_lift': lift_above_surface_m}
|
|
rospy.loginfo('Raise tool above surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
start = simple_plan[0]
|
|
forward_m = start['mobile_base_forward_m']
|
|
pose = {'translate_mobile_base': forward_m}
|
|
rospy.loginfo('Drive to the start of the cleaning region.')
|
|
self.move_to_pose(pose)
|
|
|
|
initial_wrist_position = self.wrist_position
|
|
extension_m = start['start_wrist_extension_m']
|
|
|
|
#start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
|
|
start_extension_m = initial_wrist_position + extension_m
|
|
pose = {'wrist_extension': start_extension_m}
|
|
rospy.loginfo('Extend tool above surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
use_old_code = False
|
|
if use_old_code:
|
|
self.lower_tool_until_contact()
|
|
else:
|
|
lift_m = self.lift_position - (above_surface_m - 0.02)
|
|
lift_contact_effort = 20.0 #20.0 from funmap
|
|
extension_contact_effort = 40.0 #40.0 from funmap
|
|
pose = {'joint_lift': (lift_m, lift_contact_effort)}
|
|
self.move_to_pose(pose, custom_contact_thresholds=True)
|
|
|
|
use_correction = True
|
|
if use_correction:
|
|
# raise due to drop down after contact detection
|
|
#rospy.sleep(0.5) # wait for new lift position
|
|
rospy.sleep(0.2) # wait for new lift position
|
|
lift_m = self.lift_position + 0.01 #0.015
|
|
pose = {'joint_lift': lift_m}
|
|
self.move_to_pose(pose)
|
|
rospy.sleep(0.2) # wait for new lift position
|
|
|
|
extension_m = start['end_wrist_extension_m']
|
|
#end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
|
|
end_extension_m = initial_wrist_position + extension_m
|
|
pose = {'wrist_extension': end_extension_m}
|
|
rospy.loginfo('Extend tool to end of surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
extension_m = start['start_wrist_extension_m']
|
|
pose = {'wrist_extension': start_extension_m}
|
|
rospy.loginfo('Retract tool to beginning.')
|
|
self.move_to_pose(pose)
|
|
|
|
for m in simple_plan[1:]:
|
|
forward_m = m['mobile_base_forward_m']
|
|
pose = {'translate_mobile_base': forward_m}
|
|
rospy.loginfo('Drive to the start of the cleaning region.')
|
|
self.move_to_pose(pose)
|
|
|
|
extension_m = m['start_wrist_extension_m']
|
|
#start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
|
|
start_extension_m = initial_wrist_position + extension_m
|
|
start_extension_m = max(start_extension_m, min_extension_m)
|
|
pose = {'wrist_extension': start_extension_m}
|
|
rospy.loginfo('Extend tool above surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
extension_m = m['end_wrist_extension_m']
|
|
#end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
|
|
end_extension_m = initial_wrist_position + extension_m
|
|
end_extension_m = min(end_extension_m, max_extension_m)
|
|
pose = {'wrist_extension': end_extension_m}
|
|
rospy.loginfo('Extend tool to end of surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
pose = {'wrist_extension': start_extension_m}
|
|
rospy.loginfo('Retract tool to beginning.')
|
|
self.move_to_pose(pose)
|
|
|
|
pose = {'joint_lift': lift_above_surface_m}
|
|
rospy.loginfo('Raise tool above surface.')
|
|
self.move_to_pose(pose)
|
|
|
|
pose = {'wrist_extension': initial_wrist_position}
|
|
rospy.loginfo('Retract tool to initial position.')
|
|
self.move_to_pose(pose)
|
|
|
|
return TriggerResponse(
|
|
success=True,
|
|
message='Completed surface cleaning!'
|
|
)
|
|
|
|
|
|
def main(self):
|
|
hm.HelloNode.main(self, 'clean_surface', 'clean_surface', wait_for_first_pointcloud=False)
|
|
|
|
self.debug_directory = rospy.get_param('~debug_directory')
|
|
rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory))
|
|
|
|
self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
|
|
|
|
self.trigger_clean_surface_service = rospy.Service('/clean_surface/trigger_clean_surface',
|
|
Trigger,
|
|
self.trigger_clean_surface_callback)
|
|
|
|
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)
|
|
|
|
default_service = '/camera/switch_to_default_mode'
|
|
high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
|
|
rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
|
|
rospy.wait_for_service(default_service)
|
|
rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
|
|
self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)
|
|
rospy.wait_for_service(high_accuracy_service)
|
|
rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service)
|
|
self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)
|
|
|
|
rate = rospy.Rate(self.rate)
|
|
while not rospy.is_shutdown():
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.')
|
|
args, unknown = parser.parse_known_args()
|
|
node = CleanSurfaceNode()
|
|
node.main()
|
|
except KeyboardInterrupt:
|
|
rospy.loginfo('interrupt received, so shutting down')
|