def execute(self, userdata): rospy.loginfo("[PIPELINE] Forceing robot") result = self.move_robot_communication.wait_for_result('force_robot') rospy.loginfo("[PIPELINE] Opening end effector") result = self.move_robot_communication.wait_for_result('open') rospy.loginfo("[PIPELINE] Sleep manipulator") result = self.move_robot_communication.wait_for_result('sleep') result = self.monitor_robot_communication.wait_for_result('reboot') result = self.move_robot_communication.wait_for_result( 'do_not_force_robot') # check if error is fixed result = self.monitor_robot_communication.wait_for_result( 'monitor_robot') if result != FlexGraspErrorCodes.SUCCESS: flex_grasp_error_log(result, "PIPELINE") return 'failure' result = self.move_robot_communication.wait_for_result('sleep') result = self.move_robot_communication.wait_for_result('open') result = self.move_robot_communication.wait_for_result('home') userdata.command = None userdata.prev_command = 'reset' return 'success'
def take_action(self): msg = FlexGraspErrorCodes() result = None # actions if self.command == 'monitor_robot': result = self.read_dynamixel_errors(joints='all') elif self.command == 'monitor_gripper': result = self.read_dynamixel_errors(joints='gripper') elif self.command == 'monitor_arm': result = self.read_dynamixel_errors(joints='arm') elif self.command == 'reboot': result = self.reboot() elif self.command == 'e_init': result = FlexGraspErrorCodes.SUCCESS elif self.command == None: pass else: rospy.logwarn("[%s] Received unknown command: %s", self.node_name, self.command) result = FlexGraspErrorCodes.UNKNOWN_COMMAND # publish result if result is not None: flex_grasp_error_log(result, node_name=self.node_name) msg.val = result self.pub_e_out.publish(msg) self.command = None
def take_action(self): msg = FlexGraspErrorCodes() result = None if self.command == 'e_init': result = FlexGraspErrorCodes.SUCCESS elif self.command == 'calibrate': result = self.init_poses_1() if result == FlexGraspErrorCodes.SUCCESS: result = self.calibrate() elif self.command == 'calibrate_height': result = self.init_poses_2() if result == FlexGraspErrorCodes.SUCCESS: result = self.calibrate(track_marker=False) elif self.command is not None: rospy.logwarn( "[CALIBRATE] Can not take an action: received unknown command %s!", self.command) result = FlexGraspErrorCodes.UNKNOWN_COMMAND # publish success if result is not None: msg.val = result flex_grasp_error_log(result, self.node_name) self.pub_e_out.publish(msg) self.command = None
def command_completed(self, result=FlexGraspErrorCodes.SUCCESS): """method called when the state machine completes the requested model.""" rospy.logdebug("[{0}] Completed command {1} with.".format( self.node_name, self.command)) flex_grasp_error_log(result, self.node_name) msg = FlexGraspErrorCodes(result) self.pub_e_out.publish(msg) self.reset()
def take_action(self): msg = FlexGraspErrorCodes() result = None # State dependent actions if self.state == "init" and not self.received_all_data(): if self.command == "pick": rospy.logwarn("[{0}] Can not pick object, it still needs to be detected!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR if self.command == "e_init": rospy.logdebug("[{0}] executing e_init command".format(self.node_name)) result = FlexGraspErrorCodes.SUCCESS if self.state == "idle": if self.command == "pick": result = self.pick() elif self.command == "place": rospy.logwarn("[{0}] Can not place object, it is not picked!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR elif self.state == "picked": if self.command == "place": result = self.place() elif self.command == "pick": rospy.logwarn("[{0}] Can not pick object, it still needs to be placed!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR elif self.command == "reset": result = self.reset_msg() success = (result == FlexGraspErrorCodes.SUCCESS) if result is None: success = None self.update_state(success) # publish success if result is not None: msg.val = result flex_grasp_error_log(result, self.node_name) self.pub_e_out.publish(msg) self.command = None
def execute(self, userdata): for attempt_remaining in reversed(range(self.max_attempts)): rospy.loginfo("[PIPELINE] Opening end effector") result = self.communication.wait_for_result('open') if result == FlexGraspErrorCodes.SUCCESS: rospy.loginfo("[PIPELINE] Homeing manipulator") result = self.communication.wait_for_result('home') if result == FlexGraspErrorCodes.SUCCESS: userdata.command = None userdata.prev_command = 'reset' return 'success' flex_grasp_error_log(result, "PIPELINE") rospy.logwarn("[PIPELINE] retry resetarm: %s attempts remaining", attempt_remaining) return error_handling(result)
def execute(self, userdata): rospy.logdebug('Executing state Pick Place') rospy.logdebug('Statemachine publishing command %s', userdata.command) if userdata.command == "pick_place": commands = ["pick", "place"] else: commands = [userdata.command] for command in commands: result = self.communication.wait_for_result(command) flex_grasp_error_log(result) outcome = error_handling(result) if outcome != 'success': return outcome # determine success userdata.prev_command = userdata.command userdata.command = None return outcome
def e_out_cb(self, msg): if self.result is None: if msg.val != FlexGraspErrorCodes.NONE: flex_grasp_error_log(msg.val, self.node_name, mode='debug') self.result = msg.val
def take_action(self): msg = FlexGraspErrorCodes() result = None # General actions, non state dependent if self.command == "move_manipulator": # rospy.loginfo("[MOVE ROBOT] Going to pose...") result = self.wait_for_robot_pose() if result == FlexGraspErrorCodes.SUCCESS: result = self.go_to_pose(self.robot_pose) # rospy.loginfo("[MOVE ROBOT] Result: %s", move_robot_result_string(result)) self.robot_pose = None elif self.command == "sleep": result = self.sleep_man() elif self.command == "force_robot": self.force_robot = True result = FlexGraspErrorCodes.SUCCESS elif self.command == "do_not_force_robot": self.force_robot = False result = FlexGraspErrorCodes.SUCCESS elif self.command == "home": result = self.home_man() elif self.command == "ready": result = self.ready_man() elif self.command == "grasp": result = self.apply_grasp_ee() elif self.command == "release": result = self.apply_release_ee() elif self.command == "open": result = self.apply_release_ee() elif self.command == "close": result = self.close_ee() elif self.command == "reset": result = self.reset() elif self.command == "e_init": result = FlexGraspErrorCodes.SUCCESS elif self.command == None: pass else: rospy.logwarn("Received unknown command: %s", self.command) result = FlexGraspErrorCodes.UNKNOWN_COMMAND # publish result if result is not None: flex_grasp_error_log(result, self.node_name) msg.val = result self.pub_e_out.publish(msg) self.command = None