def handle_dig_circular(req):
try:
interface = MoveGroupPythonInteface()
print "Starting full traj planning session"
dig_circular_args = activity_full_digging_traj.arg_parsing_circ(req)
if utils.check_arguments(dig_circular_args[1], dig_circular_args[2],
dig_circular_args[3]) != True:
print "[ERROR] Invalid trench input arguments. Exiting path_planning_commander..."
return
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "full_traj_"
bagname = location + currentDT
utils.start_traj_recording(dig_circular_args[6], bagname)
result = activity_full_digging_traj.dig_circular(
interface.move_arm, interface.move_limbs, dig_circular_args)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Finished planning session succesfully..."
return True, "Done"
def handle_guarded_move(req):
try:
interface = MoveGroupPythonInteface()
print "Starting guarded move planning session"
guarded_move_args = activity_guarded_move.arg_parsing(req)
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "pre_guarded_move_traj_"
bagname = location + currentDT
# Approach
utils.start_traj_recording(guarded_move_args[1], bagname)
result = activity_guarded_move.pre_guarded_move(
interface.move_arm, guarded_move_args)
utils.stop_traj_recording(result, bagname)
# Safe move, monitoring torques
location = "guarded_move_traj_"
bagname = location + currentDT
utils.start_traj_recording(False, bagname)
result = activity_guarded_move.guarded_move(interface.move_arm,
guarded_move_args)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Finished guarded move planning session succesfully..."
return True, "Done"
def handle_deliver_sample(req):
try:
interface = MoveGroupPythonInteface()
print "Starting sample delivery session"
deliver_sample_args = activity_deliver_sample.arg_parsing(req)
if utils.check_arguments(deliver_sample_args[1],
deliver_sample_args[2],
deliver_sample_args[3]) != True:
print "[ERROR] Invalid sample delivery input arguments. Exiting path_planning_commander..."
return
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "full_traj_"
bagname = location + currentDT
utils.start_traj_recording(deliver_sample_args[4], bagname)
result = activity_deliver_sample.deliver_sample(
interface.move_arm, deliver_sample_args)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Finished planning session for linear trenching succesfully..."
return True, "Done"
def handle_grind(req):
try:
interface = MoveGroupPythonInteface()
print "Starting grinder planning session"
grind_args = activity_grind.arg_parsing(req)
if utils.check_arguments(grind_args[1], grind_args[2],
grind_args[3]) != True:
print "[ERROR] Invalid grinder trajectory input arguments. Exiting path_planning_commander..."
return
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "full_traj_"
bagname = location + currentDT
utils.start_traj_recording(grind_args[6], bagname)
result = activity_grind.grind(interface.move_arm, interface.move_limbs,
interface.move_grinder, grind_args)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Grinder planning succesfully finished..."
return True, "Done"
def handle_unstow(req):
try:
interface = MoveGroupPythonInteface()
print "Moving to unstowed configuration..."
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "full_traj_"
bagname = location + currentDT
utils.start_traj_recording(req.delete_prev_traj, bagname)
result = activity_full_digging_traj.unstow(interface.move_arm)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Moved to unstowed configuration succesfully..."
return True, "Done"
def handle_stow(req):
try:
interface = MoveGroupPythonInteface()
print "Starting full traj planning session"
currentDT = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
location = "full_traj_"
bagname = location + currentDT
utils.start_traj_recording(req.delete_prev_traj, bagname)
result = activity_full_digging_traj.go_home(interface.move_arm)
utils.stop_traj_recording(result, bagname)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
print "Finished planning stow succesfully..."
return True, "Done"
