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
