Merge pull request #950 from tue-robotics/cleanup-arm

Unduplicate goal creation code
tue-robotics · Dec 14, 2019 · 474848a · 474848a
2 parents 3d7beab + bbc2cfa
commit 474848a
Showing 1 changed file with 9 additions and 19 deletions.
diff --git a/robot_skills/src/robot_skills/arms.py b/robot_skills/src/robot_skills/arms.py
@@ -913,31 +913,21 @@ def move_down_until_force_sensor_edge_up(self, timeout=10, retract_distance=0.01
         # Fill with required joint names (desired in hardware / gazebo impl)
         current_joint_state = self.get_joint_states()
         current_joint_state['arm_lift_joint'] = 0
-
-        self._ac_joint_traj.send_goal(FollowJointTrajectoryGoal(
-            trajectory=JointTrajectory(
-                joint_names=self.joint_names,
-                points=[JointTrajectoryPoint(
-                    positions=[current_joint_state[n] for n in self.joint_names],
-                    time_from_start=rospy.Duration.from_sec(timeout)
-                )]
-            )
-        ))
+        self._ac_joint_traj.send_goal(self._make_goal(current_joint_state, timeout))
 
         self.force_sensor.wait_for_edge_up(timeout)
         self.cancel_goals()
 
         current_joint_state = self.get_joint_states()
         current_joint_state['arm_lift_joint'] += retract_distance
-        self._ac_joint_traj.send_goal(FollowJointTrajectoryGoal(
-            trajectory=JointTrajectory(
-                joint_names=self.joint_names,
-                points=[JointTrajectoryPoint(
-                    positions=[current_joint_state[n] for n in self.joint_names],
-                    time_from_start=rospy.Duration.from_sec(0.5)
-                )]
-            )
-        ))
+        self._ac_joint_traj.send_goal(self._make_goal(current_joint_state, 0.5))
+
+    def _make_goal(self, current_joint_state, timeout):
+        positions = [current_joint_state[n] for n in self.joint_names]
+        points = [JointTrajectoryPoint(positions=positions,
+                                       time_from_start=rospy.Duration.from_sec(timeout))]
+        trajectory = JointTrajectory(joint_names=self.joint_names, points=points)
+        return FollowJointTrajectoryGoal(trajectory=trajectory)
 
 
 class FakeArm(RobotPart):