2 # -*- coding: iso-8859-15 -*-
9 from dynamic_reconfigure.server import Server
10 from arm_ros_conn.cfg import ArmConfig
11 from sensor_msgs.msg import JointState
12 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionResult, FollowJointTrajectoryActionFeedback, FollowJointTrajectoryResult
13 from control_msgs.msg import GripperCommandAction, GripperCommandActionResult, GripperCommandFeedback
14 from actionlib_msgs.msg import GoalStatus
15 from time import sleep
18 lJointNames = ["arm_base_to_link1", "link_1_2_joint", "link_2_3_joint", "link_3_4_joint", "link_4_5_joint", "left_gripper_joint", "right_gripper_joint"]
22 _feedback = FollowJointTrajectoryActionFeedback()
23 _result = FollowJointTrajectoryActionResult()
24 _gripper_feedback = GripperCommandFeedback()
25 _gripper_result = GripperCommandActionResult()
28 rospy.init_node('arm')
30 self.lSpeeds = [220] * 6
31 self.lAngles = [0] * 6
34 arm.set_hall_mode(3, 0)
35 arm.set_hall_mode(5, 0)
36 arm.set_tolerance(3, 0)
37 arm.set_tolerance(5, 0)
39 self._as_arm = actionlib.SimpleActionServer("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction, execute_cb=self.execute_joint_trajectory, auto_start = False)
41 self._as_gripper = actionlib.SimpleActionServer("gripper_controller/gripper_action", GripperCommandAction, execute_cb=self.execute_gripper_action, auto_start = False)
42 self._as_gripper.start()
43 self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16)
44 self.dyn_conf = Server(ArmConfig, self.execute_dyn_reconf)
49 while not rospy.is_shutdown():
50 self.publish_joint_states()
53 def execute_dyn_reconf(self, config, level):
54 self.lSpeeds = [config["speed_1"], config["speed_2"], config["speed_3"], config["speed_4"], config["speed_5"], config["speed_6"]]
57 def publish_joint_states(self):
58 joint_state = JointState()
59 joint_state.header.stamp = rospy.Time.now()
60 joint_state.name = lJointNames
61 self.lAngles = [-arm.get_angle(0), arm.get_angle(1), -arm.get_angle(2), -arm.get_angle(3), arm.get_angle(4), 0.35-arm.get_angle(5)]
62 joint_state.position = self.lAngles[:-1] + [self.lAngles[-1], self.lAngles[-1]]
63 self.pub_joint_states.publish(joint_state)
65 def execute_joint_trajectory(self, goal):
66 self._result.status = FollowJointTrajectoryResult.SUCCESSFUL
67 for point in goal.trajectory.points:
68 print goal.trajectory.joint_names
71 point.positions[goal.trajectory.joint_names.index(lJointNames[0])],
72 point.positions[goal.trajectory.joint_names.index(lJointNames[1])],
73 point.positions[goal.trajectory.joint_names.index(lJointNames[2])],
74 point.positions[goal.trajectory.joint_names.index(lJointNames[3])],
75 point.positions[goal.trajectory.joint_names.index(lJointNames[4])],
78 arm.to_angle(0, self.lSpeeds[0], -lGoalPosOrdered[0])
79 arm.to_angle(1, self.lSpeeds[1], lGoalPosOrdered[1])
80 arm.to_angle(2, self.lSpeeds[2], -lGoalPosOrdered[2])
81 arm.to_angle(3, self.lSpeeds[3], -lGoalPosOrdered[3])
82 arm.to_angle(4, self.lSpeeds[4], lGoalPosOrdered[4])
83 except arm.RangeError as e:
84 print >> sys.stderr, e.message
85 self._feedback.status = GoalStatus.REJECTED
86 self._as_arm.publish_feedback(self._feedback.feedback)
87 self._result.status = FollowJointTrajectoryResult.INVALID_GOAL
92 error = np.array(lGoalPosOrdered) - np.array(self.lAngles[:-1])
94 if all(abs(f) < 0.02 for f in error):
95 print "Position reached"
98 if self._as_arm.is_preempt_requested():
99 self._as_arm.set_preempted()
103 self._feedback.status = GoalStatus.SUCCEEDED
104 self._feedback.feedback.joint_names = lJointNames[:-1]
105 self._feedback.feedback.desired.positions = lGoalPosOrdered
106 self._feedback.feedback.actual.positions = self.lAngles[:-1]
107 self._feedback.feedback.error.positions = error
108 self._as_arm.publish_feedback(self._feedback.feedback)
109 self._as_arm.set_succeeded(self._result.result)
112 def execute_gripper_action(self, goal):
113 arm.to_angle(5, self.lSpeeds[5], 0.35-goal.command.position)
115 error = goal.command.position - self.lAngles[-1]
116 if abs(error) < 0.02:
119 self._gripper_feedback.position = self.lAngles[-1]
120 self._gripper_feedback.stalled = False
121 self._gripper_feedback.reached_goal = False
123 if self._as_gripper.is_preempt_requested():
124 self._as_gripper.set_preempted()
127 self._gripper_result.status = GoalStatus.SUCCEEDED
128 self._gripper_result.result.position = goal.command.position
129 self._gripper_result.result.stalled = False
130 self._gripper_result.result.reached_goal = True
131 self._as_gripper.set_succeeded(self._gripper_result.result)
133 if __name__ == '__main__':