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_accepted(self):
""" method called when state machine accepts the requested command """
rospy.logdebug("[%s] Accepted command in message: %s", self.NODE_NAME,
self.command)
msg = FlexGraspErrorCodes()
msg.val = FlexGraspErrorCodes.NONE
self.pub_e_out.publish(msg)
def e_in_cb(self, msg):
if self.command is None:
self.command = msg.data
rospy.logdebug("[{0}] Received command in message: {1}".format(self.node_name, self.command))
# reset outputting message
msg = FlexGraspErrorCodes()
msg.val = FlexGraspErrorCodes.NONE
self.pub_e_out.publish(msg)
def e_in_cb(self, msg):
if self.command is None:
self.command = msg.data
rospy.logdebug("[MOVE ROBOT] Received new move robot event in message: %s", self.command)
# reset outputting message
msg = FlexGraspErrorCodes()
msg.val = FlexGraspErrorCodes.NONE
self.pub_e_out.publish(msg)
def e_in_cb(self, msg):
if self.command is None:
self.command = msg.data
rospy.logdebug("[%s] Received new command: %s", self.node_name,
self.command)
# reset outputting message
msg = FlexGraspErrorCodes()
msg.val = FlexGraspErrorCodes.NONE
self.pub_e_out.publish(msg)
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 command_rejected(self, error_code=FlexGraspErrorCodes.FAILURE):
"""method called when the state machine rejects the requested model."""
rospy.logdebug("[%s] Rejected command in message", self.node_name)
msg = FlexGraspErrorCodes(error_code)
self.pub_e_out.publish(msg)
self.reset()
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