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"