Use services to set position, or speed and position instead of topics

dynamixel_controllers/CMakeLists.txt

@@ -9,7 +9,9 @@ add_service_files(
   SetComplianceMargin.srv
   SetCompliancePunch.srv
   SetComplianceSlope.srv
+  SetPosition.srv
   SetSpeed.srv
+  SetSpeedandPosition.srv
   SetTorqueLimit.srv
   StartController.srv
   StopController.srv

dynamixel_controllers/src/dynamixel_controllers/joint_controller.py

@@ -47,7 +47,9 @@
 
 from dynamixel_driver.dynamixel_const import *
 
+from dynamixel_controllers.srv import SetPosition
 from dynamixel_controllers.srv import SetSpeed
+from dynamixel_controllers.srv import SetSpeedandPosition
 from dynamixel_controllers.srv import TorqueEnable
 from dynamixel_controllers.srv import SetComplianceSlope
 from dynamixel_controllers.srv import SetComplianceMargin
@@ -74,6 +76,8 @@ def __init__(self, dxl_io, controller_namespace, port_namespace):
         self.__ensure_limits()
 
         self.speed_service = rospy.Service(self.controller_namespace + '/set_speed', SetSpeed, self.process_set_speed)
+        self.position_service = rospy.Service(self.controller_namespace + '/set_position', SetPosition, self.process_set_position)
+        self.speed_position_service = rospy.Service(self.controller_namespace + '/set_speed_and_position', SetSpeedandPosition, self.process_set_speed_and_position)
         self.torque_service = rospy.Service(self.controller_namespace + '/torque_enable', TorqueEnable, self.process_torque_enable)
         self.compliance_slope_service = rospy.Service(self.controller_namespace + '/set_compliance_slope', SetComplianceSlope, self.process_set_compliance_slope)
         self.compliance_marigin_service = rospy.Service(self.controller_namespace + '/set_compliance_margin', SetComplianceMargin, self.process_set_compliance_margin)
@@ -115,6 +119,7 @@ def stop(self):
         self.motor_states_sub.unregister()
         self.command_sub.unregister()
         self.speed_service.shutdown('normal shutdown')
+        self.position_service.shutdown('normal shutdown')
         self.torque_service.shutdown('normal shutdown')
         self.compliance_slope_service.shutdown('normal shutdown')
 
@@ -124,6 +129,9 @@ def set_torque_enable(self, torque_enable):
     def set_speed(self, speed):
         raise NotImplementedError
 
+    def set_position(self, position):
+        raise NotImplementedError
+
     def set_compliance_slope(self, slope):
         raise NotImplementedError
 
@@ -140,6 +148,13 @@ def process_set_speed(self, req):
         self.set_speed(req.speed)
         return [] # success
 
+    def process_set_speed_and_position(self, req):
+        self.set_speed(req.speed)
+        return self.set_position(req.position)
+
+    def process_set_position(self, req):
+        return self.set_position(req.position)
+
     def process_torque_enable(self, req):
         self.set_torque_enable(req.torque_enable)
         return []

dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

@@ -132,6 +132,27 @@ def set_speed(self, speed):
         mcv = (self.motor_id, self.spd_rad_to_raw(speed))
         self.dxl_io.set_multi_speed([mcv])
 
+    def set_position(self, position):
+        goal_reached = False
+        min_max_limit_reached = False
+        angle = position
+        limit = 0.1
+        mcv = (self.motor_id, self.pos_rad_to_raw(angle))
+        self.dxl_io.set_multi_position([mcv])
+
+        # Without this sleep we fail to get self.joint_state.is_moving as it is still False.
+        rospy.sleep(0.1)
+
+        while self.joint_state.is_moving:
+            rospy.sleep(0.1)
+
+        if abs(self.joint_state.current_pos - position) < limit:
+            goal_reached = True
+        if abs(self.max_angle - self.joint_state.goal_pos) < limit or \
+                        abs(self.min_angle - self.joint_state.goal_pos) < limit:
+            min_max_limit_reached = True
+        return (goal_reached, min_max_limit_reached)
+
     def set_compliance_slope(self, slope):
         if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
         elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE

dynamixel_controllers/srv/SetPosition.srv

@@ -0,0 +1,5 @@
+float64 position
+---
+bool goal_reached
+bool min_max_limit_reached
+

dynamixel_controllers/srv/SetSpeedandPosition.srv

@@ -0,0 +1,6 @@
+float64 speed
+float64 position
+---
+bool goal_reached
+bool min_max_limit_reached
+