class arm_server_node(object):
"""
This node is an action server, to help simplify arm movement,
using moveit_commander
"""
_feedback = elevator.msg.SimpleTargetFeedback()
_result = elevator.msg.SimpleTargetResult()
def __init__(self, name):
# wait for moveit
while not "/move_group/result" in dict(
rospy.get_published_topics()).keys():
rospy.sleep(2)
self.group = MoveGroupCommander("arm")
self.group.set_start_state_to_current_state()
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_pose_reference_frame("/base_footprint")
self.group.set_max_velocity_scaling_factor(1)
self.group.set_num_planning_attempts(50)
self.group.set_planning_time(10)
self.group.set_goal_position_tolerance(0.01)
self.group.set_goal_orientation_tolerance(0.02)
self.tf_listener = TransformListener()
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
elevator.msg.SimpleTargetAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def execute_cb(self, goal):
eef_pose = self.group.get_current_pose("gripper_link")
self._feedback.x = math.fabs(goal.x - eef_pose.pose.position.x)
self._feedback.y = math.fabs(goal.y - eef_pose.pose.position.y)
self._feedback.z = math.fabs(goal.z - eef_pose.pose.position.z)
self._feedback.distance = math.sqrt(
math.pow(self._feedback.x, 2) + math.pow(self._feedback.y, 2) +
math.pow(self._feedback.z, 2))
# publish the feedback
self._as.publish_feedback(self._feedback)
# start executing the action
if goal.frame_id[0] != '/':
rospy.loginfo('[%s]: Executing, moving arm to "%s" pose' %
(self._action_name, goal.frame_id))
self.group.set_named_target(goal.frame_id)
named_plan = self.group.plan()
self.group.execute(named_plan)
else:
rospy.loginfo(
'[%s]: Executing, moving gripper from (%.3f, %.3f, %.3f) to (%.3f, %.3f, %.3f)'
% (self._action_name, eef_pose.pose.position.x,
eef_pose.pose.position.y, eef_pose.pose.position.z, goal.x,
goal.y, goal.z))
origin_goal = PoseStamped()
origin_goal.header.frame_id = "/gripper_link"
origin_goal.pose.position.x = goal.x
origin_goal.pose.position.y = goal.y
origin_goal.pose.position.z = goal.z
transformed_goal = self.tf_listener.transformPose(
"/base_footprint", origin_goal)
transformed_goal.pose.orientation.x = 0
transformed_goal.pose.orientation.y = 0
transformed_goal.pose.orientation.z = 0
transformed_goal.pose.orientation.w = 0
self.group.set_pose_target(transformed_goal)
plan = self.group.plan()
self.group.execute(plan)
