From 8d3ff7b5cdaff9ad5cf87b74b0b584722cef0a62 Mon Sep 17 00:00:00 2001
From: Nils Schulte <git@nilsschulte.de>
Date: Tue, 13 Sep 2022 12:15:54 +0200
Subject: [PATCH] try restart motor on error

---
 .../dynamixel_driver.hpp                      | 31 ++++++++++++++-----
 .../dynamixel_ros2_controll.hpp               |  3 ++
 src/dynamixel_ros2_control.cpp                |  6 ++++
 3 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/include/dynamixel_ros2_control/dynamixel_driver.hpp b/include/dynamixel_ros2_control/dynamixel_driver.hpp
index f70f80b..8d9c4d1 100644
--- a/include/dynamixel_ros2_control/dynamixel_driver.hpp
+++ b/include/dynamixel_ros2_control/dynamixel_driver.hpp
@@ -54,6 +54,7 @@ class Driver {
 		//double goal_velocity;
 		double velocity;
 		double effort;
+		bool torque_target;
 		bool torque;
 
 		uint8_t hw_error;
@@ -101,6 +102,7 @@ class Driver {
 				rebooted(false), 
 				goal_position(std::numeric_limits<double>::quiet_NaN()), 
 				position(std::numeric_limits<double>::quiet_NaN()), 
+				torque_target(false),
 				p_gain(std::numeric_limits<double>::quiet_NaN()), 
 				i_gain(std::numeric_limits<double>::quiet_NaN()), 
 				d_gain(std::numeric_limits<double>::quiet_NaN()) 
@@ -127,9 +129,9 @@ class Driver {
 				command::Torque_Enable,
 				[this](uint32_t data) {
 					this->torque = bool(data);
-					if (this->torque == false) {
-						goal_position = std::numeric_limits<double>::quiet_NaN();
-					}
+					//if (this->torque == false) {
+						//goal_position = std::numeric_limits<double>::quiet_NaN();
+					//}
 				});
 
 			//Present Position
@@ -321,9 +323,11 @@ class Driver {
 	 * to prepare it for the writeing over the bus.
 	 */
 	void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
+		motor->torque_target = torque;
 		if(torque && motor->hw_error) {
-			if(motor->rebooting) 
+			if(motor->rebooting) {
 				return;
+			}
 			throw hardware_error(motor, "Can't enable torque!");
 		}
 		field command_field;
@@ -331,13 +335,19 @@ class Driver {
 		uint32_t p = torque;
 		add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
 	}
-
+	
     void reboot(dynamixel::Driver::Motor::Ptr& motor) {
 		motor->after_reboot_functions.clear();
+		//bool old_torque_target = motor->torque_target;
 		motor->after_reboot_functions.push_back([this, motor](){
+			motor->torque = true;
+			set_torque(motor, false);
 			this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
 		});
 		packetHandler->reboot(portHandler.get(),motor->id);
+		set_torque(motor, false);
+		motor->torque = false;
+		write();
 	}
 
 	hw_error_callback overload_error_callback;
@@ -450,6 +460,9 @@ class Driver {
 	 */
 	void write(bool enable_torque = false) {
 		for (const auto& [joint_name, motor]: motors) {
+			if(motor->torque_target != motor->torque) {
+				set_torque(motor, motor->torque_target);
+			}
 			if(motor->torque || enable_torque) {
 				if(std::isnan(motor->goal_position))
 					motor->goal_position = motor->position;
@@ -502,6 +515,9 @@ class Driver {
 						after_reboot_f();
 					}
 				}
+				if(motor->rebooted) {
+					motor->after_reboot_functions.clear();
+				}
 			}
 		}
 	}
@@ -670,9 +686,10 @@ class Driver {
 		auto motor = it_motor->second;	//motors[joint_name];
 
 		if (!motor->torque) {
-			if (enable_torque)
+			if (enable_torque) {
 				set_torque(motor, true);
-			else
+				return;
+			} else
 				throw torque_not_enabled(motor);
 		}
 
diff --git a/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp b/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp
index d408148..f960db3 100644
--- a/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp
+++ b/include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp
@@ -18,6 +18,9 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface)
 
+  DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
+  DynamixelHardwareInterface();
+
   DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
   CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
 
diff --git a/src/dynamixel_ros2_control.cpp b/src/dynamixel_ros2_control.cpp
index 874821a..d3b6b14 100644
--- a/src/dynamixel_ros2_control.cpp
+++ b/src/dynamixel_ros2_control.cpp
@@ -47,6 +47,12 @@
 #define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0"
 
 namespace dynamixel_ros2_control {
+DynamixelHardwareInterface::DynamixelHardwareInterface() {
+  rclcpp::on_shutdown([&](){
+    dynamixel_driver_->set_torque_all(false);
+    dynamixel_driver_->write();
+  });
+}
 
 CallbackReturn DynamixelHardwareInterface::on_init(
     const hardware_interface::HardwareInfo &info) {