Added an Enable service to turn a servo "on" or "off" so it ignores position requests

dynamixel_controllers/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation)
 
 add_service_files(
   FILES
+  Enable.srv
   RestartController.srv
   SetComplianceMargin.srv
   SetCompliancePunch.srv

dynamixel_controllers/src/dynamixel_controllers/joint_controller.py

@@ -49,6 +49,7 @@
 
 from dynamixel_controllers.srv import SetSpeed
 from dynamixel_controllers.srv import TorqueEnable
+from dynamixel_controllers.srv import Enable
 from dynamixel_controllers.srv import SetComplianceSlope
 from dynamixel_controllers.srv import SetComplianceMargin
 from dynamixel_controllers.srv import SetCompliancePunch
@@ -75,6 +76,7 @@ def __init__(self, dxl_io, controller_namespace, port_namespace):
 
         self.speed_service = rospy.Service(self.controller_namespace + '/set_speed', SetSpeed, self.process_set_speed)
         self.torque_service = rospy.Service(self.controller_namespace + '/torque_enable', TorqueEnable, self.process_torque_enable)
+        self.enable_service = rospy.Service(self.controller_namespace + '/enable', Enable, self.process_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)
         self.compliance_punch_service = rospy.Service(self.controller_namespace + '/set_compliance_punch', SetCompliancePunch, self.process_set_compliance_punch)
@@ -120,6 +122,9 @@ def stop(self):
 
     def set_torque_enable(self, torque_enable):
         raise NotImplementedError
+
+    def set_enable(self, enable):
+        raise NotImplementedError
 
     def set_speed(self, speed):
         raise NotImplementedError
@@ -143,6 +148,10 @@ def process_set_speed(self, req):
     def process_torque_enable(self, req):
         self.set_torque_enable(req.torque_enable)
         return []
+
+    def process_enable(self, req):
+        self.set_enable(req.enable)
+        return []
 
     def process_set_compliance_slope(self, req):
         self.set_compliance_slope(req.slope)

dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

@@ -104,6 +104,8 @@ def initialize(self):
 
         self.set_speed(self.joint_speed)
 
+        self.enabled = True
+
         return True
 
     def pos_rad_to_raw(self, pos_rad):
@@ -120,6 +122,9 @@ def spd_rad_to_raw(self, spd_rad):
     def set_torque_enable(self, torque_enable):
         mcv = (self.motor_id, torque_enable)
         self.dxl_io.set_multi_torque_enabled([mcv])
+
+    def set_enable(self, enable):
+        self.enabled = enable
 
     def set_speed(self, speed):
         mcv = (self.motor_id, self.spd_rad_to_raw(speed))
@@ -169,7 +174,8 @@ def process_motor_states(self, state_list):
                 self.joint_state_pub.publish(self.joint_state)
 
     def process_command(self, msg):
-        angle = msg.data
-        mcv = (self.motor_id, self.pos_rad_to_raw(angle))
-        self.dxl_io.set_multi_position([mcv])
+        if self.enabled:
+            angle = msg.data
+            mcv = (self.motor_id, self.pos_rad_to_raw(angle))
+            self.dxl_io.set_multi_position([mcv])
 

dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py

@@ -122,8 +122,8 @@ def set_speed(self, speed):
         elif speed > self.joint_max_speed: speed = self.joint_max_speed
         self.last_commanded_torque = speed
         speed_raw = int(round(speed / self.VELOCITY_PER_TICK))
-        mcv_master = (self.master_id, speed_raw)
-        mcv_slave = (self.slave_id, -mcv_master[1])
+        mcv_master = (self.master_id, speed_raw if speed_raw > 0 else 1)
+        mcv_slave = (self.slave_id, mcv_master[1])
         self.dxl_io.set_multi_speed([mcv_master, mcv_slave])
 
 

dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py

@@ -158,6 +158,14 @@ def process_trajectory(self, traj):
             rospy.logerr(msg)
             self.action_server.set_aborted(text=msg)
             return
+
+        # make sure none of the required servos is disabled
+        for joint in self.joint_names:
+            if not self.joint_to_controller[joint].enabled:
+                msg = 'At least one of the required servos is currently disabled'
+                rospy.logerr(msg)
+                self.action_server.set_aborted(text=msg) 
+                return               
 
         # correlate the joints we're commanding to the joints in the message
         # map from an index of joint in the controller to an index in the trajectory

dynamixel_controllers/srv/Enable.srv

@@ -0,0 +1,3 @@
+bool enable
+---
+