class Pick(object):
_feedback = moveit_msgs.msg.PickupActionFeedback().feedback
_result = moveit_msgs.msg.PickupActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PickupAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Pick action is ok. Awaiting for connections')
def get_target(self, target_name):
obj = self.scene.get_objects([target_name])
obj = obj[target_name]
pose = obj.primitive_poses[0].position
target = [0.17, 0.005, 0.019]
rospy.loginfo(target)
target = [pose.x, pose.y, pose.z]
rospy.loginfo(target)
return target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
#self._result.trajectory_start = self.robot.robot.get_current_state()
self.robot.set_start_state_to_current_state()
self._feedback.state = "Open Gripper"
self._as.publish_feedback(self._feedback)
self.robot.robot.get_current_state()
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Opening gripper"
print self._feedback
self.robot.gripper_execute(plan)
self._as.publish_feedback(self._feedback)
rospy.sleep(1)
self._feedback.state = "Planning to reach object"
self._as.publish_feedback(self._feedback)
target = self.get_target(goal.target_name)
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to grasp failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Going to the object"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to grasp the object")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
self._feedback.state = "Removing obtect to be grasp from the planning scene"
self._as.publish_feedback(self._feedback)
obj = self.scene.get_objects([goal.target_name])
obj = obj[goal.target_name]
self.scene.remove_world_object(goal.target_name)
rospy.sleep(1)
self._feedback.state = "Planning to close the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.closeGripper()
if plan is None:
rospy.loginfo("Close Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('CloseGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Closing gripper"
print self._feedback
self.robot.gripper_execute(plan)
rospy.sleep(1)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Attaching object"
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.attach_box(
"gripper_link", obj.id, pose, obj.primitives[0].dimensions,
['gripper_link', 'gripper_active_link', 'gripper_active2_link'])
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
rospy.loginfo(self._result)
self._as.set_succeeded(self._result)
class Pick(object):
_feedback = moveit_msgs.msg.PickupActionFeedback().feedback
_result = moveit_msgs.msg.PickupActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PickupAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Pick action is ok. Awaiting for connections')
def get_target(self, target_name):
obj = self.scene.get_objects([target_name])
obj = obj[target_name]
pose = obj.primitive_poses[0].position
target = [pose.x, pose.y, pose.z]
size = obj.primitives[0].dimensions
rospy.loginfo(target)
return size, target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
#self._result.trajectory_start = self.robot.robot.get_current_state()
self.robot.set_start_state_to_current_state()
self._feedback.state = "Open Gripper"
self._as.publish_feedback(self._feedback)
self.robot.robot.get_current_state()
rospy.loginfo('Open Gripper Plan')
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Opening gripper"
rospy.loginfo('Opening gripper')
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to open gripper failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
self.scene.remove_attached_object('gripper_link')
self._feedback.state = "Planning to reach object"
rospy.loginfo('Planning to reach obj')
self._as.publish_feedback(self._feedback)
dimension, target = self.get_target(goal.target_name)
quat = []
if len(goal.possible_grasps) > 0:
quat = [
goal.possible_grasps[0].grasp_pose.pose.orientation.x,
goal.possible_grasps[0].grasp_pose.pose.orientation.y,
goal.possible_grasps[0].grasp_pose.pose.orientation.z,
goal.possible_grasps[0].grasp_pose.pose.orientation.w
]
rospy.loginfo('Pick Quaternion [%s]', quat)
plan = self.robot.ef_pose(target, dimension, orientation=quat)
if plan is None:
rospy.loginfo("Plan to grasp failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._feedback.state = "Going to the object"
rospy.loginfo('Going to Obj')
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to grasp the object")
self._result.trajectory_stages.append(plan)
ex_status = self.robot.arm_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
# self._feedback.state = "Removing obtect to be grasp from the planning scene"
# self._as.publish_feedback(self._feedback)
obj = self.scene.get_objects([goal.target_name])
obj = obj[goal.target_name]
# self.scene.remove_world_object(goal.target_name)#
self._feedback.state = "Attaching object"
self._as.publish_feedback(self._feedback)
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.attach_box(
"gripper_link", obj.id, pose, obj.primitives[0].dimensions,
['gripper_link', 'gripper_active_link', 'gripper_active2_link'])
rospy.sleep(1)
rospy.sleep(1)
self._feedback.state = "Planning to close the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.closeGripper()
if plan is None:
rospy.loginfo("Close Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('CloseGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Closing gripper"
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
self._as.set_succeeded(self._result)
