[RFC] [WIP] Add feature to reboot using a service call

CMakeLists.txt

@@ -13,8 +13,24 @@ find_package(catkin REQUIRED COMPONENTS
   controller_manager
 )
 
+add_message_files(
+  FILES
+  ServoID.msg
+)
+
+add_service_files(
+  FILES
+  Reboot.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  dynamixel_control_hw
+)
+
 catkin_package(
- CATKIN_DEPENDS roscpp std_msgs message_runtime
+  CATKIN_DEPENDS roscpp std_msgs message_runtime
 )
 
 if ("$ENV{RESIBOTS_DIR}" STREQUAL "" AND "${LIBDYNAMIXEL}" STREQUAL "")

include/dynamixel_control_hw/dynamixel_hardware_interface.hpp

@@ -89,6 +89,13 @@ namespace dynamixel {
         **/
         void write_joints();
 
+        /** Send a reboot command to a dynamixel
+
+            takes the target position from memory (given by a controller) and sends
+            them to the dynamixels.
+        **/
+        bool reboot_joint(const id_t& id);
+
     private:
         using dynamixel_servo = std::shared_ptr<dynamixel::servos::BaseServo<Protocol>>;
 
@@ -384,6 +391,44 @@ namespace dynamixel {
         }
     }
 
+    template <>
+    bool DynamixelHardwareInterface<Protocol1>::reboot_joint(const id_t& id)
+    {
+        // Reboot command not implemented in Protocol1
+        return false;
+    }
+
+    template <typename Protocol>
+    bool DynamixelHardwareInterface<Protocol>::reboot_joint(const id_t& id)
+    {
+        auto servo_it = std::find_if(_servos.begin(), _servos.end(),
+            [&id](const dynamixel_servo& s) {
+                return s->id() == id;
+            });
+        if (servo_it == _servos.end()) {
+            ROS_ERROR_STREAM("Cannot reboot non-existent ID: "
+                             << static_cast<int>(id));
+            return false;
+        }
+        auto& servo = *servo_it;
+
+        ROS_INFO_STREAM("Rebooting and enabling ID: "
+                        << static_cast<int>(id));
+
+        dynamixel::StatusPacket<Protocol2> status;
+        // Send reboot
+        dynamixel::instructions::Reboot<Protocol2> packet(id);
+        _dynamixel_controller.send(packet);
+        _dynamixel_controller.recv(status);
+
+        ros::Duration{0.1}.sleep();
+
+        // Enable
+        _dynamixel_controller.send(servo->set_torque_enable(1));
+        _dynamixel_controller.recv(status);
+        return true;
+    }
+
     /** Serach for the requested servos
 
         Servos that were not requested are ignored and the software complain if

include/dynamixel_control_hw/dynamixel_loop.hpp

@@ -50,6 +50,9 @@
 // ROS control
 #include <controller_manager/controller_manager.h>
 
+// Service to reboot (only for Protocol2)
+#include <dynamixel_control_hw/Reboot.h>
+
 // The hardware interface to dynamixels
 #include <dynamixel_control_hw/dynamixel_hardware_interface.hpp>
 
@@ -88,6 +91,24 @@ namespace dynamixel {
             // Start timer that will periodically call DynamixelLoop::update
             _desired_update_freq = ros::Duration(1 / _loop_hz);
             _non_realtime_loop = _nh.createTimer(_desired_update_freq, &DynamixelLoop::update, this);
+            _service_server = _nh.advertiseService("reboot", &DynamixelLoop::reboot_joints, this);
+        }
+
+        bool reboot_joints(dynamixel_control_hw::Reboot::Request& req,
+                           dynamixel_control_hw::Reboot::Response& res)
+        {
+            bool all_success = true;
+            for (const auto& servo_id : req.servos) {
+                auto id = static_cast<typename hw_int::id_t>(servo_id.id);
+
+                dynamixel_access_mutex.lock();
+                auto result = _hardware_interface->reboot_joint(id);
+                dynamixel_access_mutex.unlock();
+
+                res.results.push_back(result);
+                all_success = all_success && result;
+            }
+            return all_success;
         }
 
         /** Timed method that reads current hardware's state, runs the controller
@@ -117,15 +138,19 @@ namespace dynamixel {
 
             // Input
             // get the hardware's state
+            dynamixel_access_mutex.lock();
             _hardware_interface->read_joints();
+            dynamixel_access_mutex.unlock();
 
             // Control
             // let the controller compute the new command (via the controller manager)
             _controller_manager->update(ros::Time::now(), _elapsed_time);
 
             // Output
             // send the new command to hardware
+            dynamixel_access_mutex.lock();
             _hardware_interface->write_joints();
+            dynamixel_access_mutex.unlock();
         }
 
     private:
@@ -142,6 +167,8 @@ namespace dynamixel {
         double _loop_hz;
         steady_clock::time_point _last_time;
         steady_clock::time_point _current_time;
+        boost::mutex dynamixel_access_mutex;
+        ros::ServiceServer _service_server;
 
         /** ROS Controller Manager and Runner
 

msg/ServoID.msg

@@ -0,0 +1 @@
+uint8 id

srv/Reboot.srv

@@ -0,0 +1,3 @@
+dynamixel_control_hw/ServoID[] servos
+---
+bool[] results