auto reset on hardware error

README.md

@@ -7,16 +7,15 @@ are welcome too.
 Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed.
 Clone it in your workspace and checkout the branch ros2:
 ```
-git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
+git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git -b ros2
 cd DynamixelSDK/
-git checkout ros2
 ```
 finally build with colcon
 `colcon build`
 
 ## Configure Dynamixel motor parameters via URDF
 
-Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part of your `.urdf` / `.xarco` files.
+Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part of your `.urdf` / `.xarco` files. 
 
 ```xml
 <ros2_control name="my_robot" type="system">
@@ -27,6 +26,7 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
       </hardware>
       <joint name="joint1">
         <param name="id">10</param>
+        <!--param name="reboot_on_error">true</param-->
         <command_interface name="position"/>
         <!--command_interface name="velocity"/-->
         <state_interface name="position"/>
@@ -35,6 +35,8 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
       </joint>
       ...
 ```
+When `reboot_on_error` is "true" the motor automaticaly reboots on mechanical overload error (it still loses torque for a brief moment, but that should be enogh to let the motor recover the fault).
+
 ## Run a test
 You need the urdf of the robot with the ros2_control xml added.
 After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators:

include/dynamixel_ros2_control/dynamixel_driver.hpp

@@ -41,6 +41,7 @@ class Driver {
 		const model_info* model;
 
 		bool led;
+		bool rebooting;
 		
 		double goal_position;
 
@@ -86,7 +87,7 @@ class Driver {
 		void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); }
 		void set_effort_from_raw(uint32_t raw) { effort = double(static_cast<int16_t>(raw))*0.1; }
 
-		Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), goal_position(std::nan("")), position(std::nan("")){
+		Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), rebooting(false), goal_position(std::nan("")), position(std::nan("")) {
 			try {
 				model = &model_infos.at(type);
 			} catch (const std::exception &e) {
@@ -173,7 +174,9 @@ class Driver {
 								error_callback(hardware_status::Overload_Error);
 							}
 							//https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status
-						}	
+						} else {
+							this->rebooting = false;
+						}
 					});
 			}
 
@@ -272,6 +275,8 @@ class Driver {
 	 */
 	void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
 		if(torque && motor->hw_error) {
+			if(motor->rebooting) 
+				return;
 			throw hardware_error(motor, "Can't enable torque!");
 		}
 		field command_field;
@@ -606,6 +611,7 @@ class Driver {
 		auto it_motor = motors.find(joint_name);
 		if (it_motor == motors.end())
 			throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!");
+		it_motor->second->rebooting = true;
 		reboot(it_motor->second);
 	}
 };

src/dynamixel_ros2_control.cpp

@@ -148,7 +148,7 @@ void DynamixelHardwareInterface::overload_error_callback(
     try {
       dynamixel_driver_->reboot(joint_name);
       RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
-                  ("\"" + joint_name + " rebootet").c_str());
+                  ("\"" + joint_name + "\" rebooting").c_str());
       // succeeded = true;
     } catch (std::invalid_argument &e) {
       RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what());