Fixed resetting joint states when performing mode switches (#11)

* Fixed resetting joint states when performing mode switches

* Improved Logging in rest goal state

* Logging Improvement Mode Switching to None

* Disabled respawning of controller manager

Author: Joschi3

dynamixel_ros_control/include/dynamixel_ros_control/joint.hpp

@@ -37,6 +37,7 @@ class Joint
   bool addActiveCommandInterface(const std::string& interface_name);
   bool removeActiveCommandInterface(const std::string& interface_name);
   bool updateControlMode();
+  bool readControlMode();
   bool controlModeChanged() const;
   void resetControlModeChanged();
 

dynamixel_ros_control/launch/controller_manager.launch.yaml

@@ -30,8 +30,8 @@ launch:
     name: "joint_state_broadcaster_spawner"
     output: "screen"
     args: "joint_state_broadcaster"
-    respawn: "true"
-    respawn_delay: 5.0
+    #respawn: "true"
+   # respawn_delay: 5.0
 
 - node:
     pkg: "controller_manager"

dynamixel_ros_control/src/dynamixel_hardware_interface.cpp

@@ -139,6 +139,11 @@ DynamixelHardwareInterface::on_init(const hardware_interface::HardwareInfo& hard
   set_torque_service_ = node_->create_service<std_srvs::srv::SetBool>(
       "~/set_torque", [this](const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
                              const std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
+        if (lifecycle_state_.label() != hardware_interface::lifecycle_state_names::ACTIVE) {
+          response->success = false;
+          response->message = "Hardware Interface must be in 'active' state to set torque";
+          return;
+        }
         DXL_LOG_INFO("Request to set torque to " << (request->data ? "ON" : "OFF") << " received.");
         response->success = setTorque(request->data);
         response->message = response->success ? "Torque set successfully" : "Failed to set torque";
@@ -152,8 +157,13 @@ DynamixelHardwareInterface::on_init(const hardware_interface::HardwareInfo& hard
 
   // set up e-stop subscription
   soft_e_stop_subscription_ = node_->create_subscription<std_msgs::msg::Bool>(
-      "~/soft_e_stop", rclcpp::SystemDefaultsQoS(),
-      [this](const std_msgs::msg::Bool::SharedPtr msg) { setEStop(msg->data); });
+      "~/soft_e_stop", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Bool::SharedPtr msg) {
+        if (lifecycle_state_.label() != hardware_interface::lifecycle_state_names::ACTIVE) {
+          DXL_LOG_WARN("E-Stop message received but hardware interface is not in 'active' state.");
+          return;
+        }
+        setEStop(msg->data);
+      });
   // Transmissions
   if (!loadTransmissionConfiguration()) {
     return hardware_interface::CallbackReturn::ERROR;
@@ -242,7 +252,14 @@ hardware_interface::CallbackReturn DynamixelHardwareInterface::on_activate(const
     DXL_LOG_ERROR("Failed to set torque on activation to " << (torque_on_startup_ ? "ON" : "OFF"));
     return hardware_interface::CallbackReturn::ERROR;
   }
-  is_torqued_ = torque_on_startup_;  // TODO: check if successful
+  // make sure position control mode is active (safer than leaving it in whatever mode it was before)
+  // imagine, torque on startup but actuator from last shutdown in current mode & no controller running
+  for (auto& [name, joint] : joints_) {
+    if (!joint.readControlMode() || !joint.updateControlMode()) {
+      return CallbackReturn::ERROR;
+    }
+  }
+  is_torqued_ = torque_on_startup_;
   if (!resetGoalStateAndVerify()) {
     return CallbackReturn::ERROR;
   }
@@ -384,6 +401,9 @@ DynamixelHardwareInterface::perform_command_mode_switch(const std::vector<std::s
       return hardware_interface::return_type::ERROR;
     }
   }
+
+  first_read_successful_ = false;  // force second reset in read
+
   mode_switch_failed_ = false;  // mark as successful
   return hardware_interface::return_type::OK;
 }
@@ -743,7 +763,7 @@ bool DynamixelHardwareInterface::resetGoalStateAndVerify()
 {
   // Read current values (positions, velocities, etc.) before enabling torque
   if (!read_manager_.read() || !read_manager_.isOk() || !isHardwareOk()) {
-    DXL_LOG_ERROR("Failed to read current positions before enabling torque. Cannot enable torque.");
+    DXL_LOG_ERROR("[resetGoalStateAndVerify] Failed to read current values from actuators.");
     return false;
   }
 
@@ -754,29 +774,30 @@ bool DynamixelHardwareInterface::resetGoalStateAndVerify()
 
   // Write goal positions (will only write for the values belonging to the active command interfaces!)
   if (!control_write_manager_.write() || !control_write_manager_.isOk() || !isHardwareOk()) {
-    DXL_LOG_ERROR("Failed to write goal positions before enabling torque. Cannot enable torque.");
+    DXL_LOG_ERROR("[resetGoalStateAndVerify] Failed to write reset goal values.");
     return false;
   }
 
   // Re-read goal values for verification
   if (!cmd_read_manager_.read() || !cmd_read_manager_.isOk()) {
-    DXL_LOG_ERROR("Failed to re-read goal positions before enabling torque. Cannot verify goal positions.");
+    DXL_LOG_ERROR("[resetGoalStateAndVerify] Failed to re-read goal.");
     return false;
   }
 
   // Verify goal command values match the read values (for active command interfaces)
   for (auto& [name, joint] : joints_) {
     for (const auto& interface_name : joint.getAvailableCommandInterfaces()) {
       if (joint.read_goal_values_.count(interface_name) == 0) {
-        DXL_LOG_ERROR("Cannot verify cmd values from motor " << name << "!");
+        DXL_LOG_ERROR("[resetGoalStateAndVerify]  Cannot verify cmd values from motor " << name << "!");
         return false;
       }
       const auto& interface_value = joint.read_goal_values_.at(interface_name);
       if (std::abs(interface_value - joint.getActuatorState().goal[interface_name]) > 1e-2) {
-        DXL_LOG_ERROR("Joint '" << name << "' goal " << interface_name
-                                << " does not match read goal position before enabling torque. "
-                                << "(Current: " << joint.getActuatorState().goal[interface_name]
-                                << ", Read Goal Position: " << interface_value << ")");
+        DXL_LOG_ERROR("[resetGoalStateAndVerify] Joint '"
+                      << name << "' goal " << interface_name
+                      << " does not match read goal value before enabling torque. "
+                      << "(Current: " << joint.getActuatorState().goal[interface_name]
+                      << ", Read Goal Value: " << interface_value << ")");
         return false;
       }
     }

dynamixel_ros_control/src/joint.cpp

@@ -166,10 +166,25 @@ bool Joint::removeActiveCommandInterface(const std::string& interface_name)
   return true;
 }
 
+bool Joint::readControlMode()
+{
+  int32_t mode = UNDEFINED;
+  if (!dynamixel->readRegister(DXL_REGISTER_CONTROL_MODE, mode)) {
+    return false;
+  }
+  control_mode_ = static_cast<ControlMode>(mode);
+  return true;
+}
+
 bool Joint::updateControlMode()
 {
   if (active_command_interfaces_.empty()) {
-    // Do nothing if no command interface is active
+    // Switch to position mode if no interface is active
+    if (control_mode_ != POSITION && control_mode_ != EXTENDED_POSITION && control_mode_ != CURRENT_BASED_POSITION) {
+      control_mode_ = preferred_position_control_mode_;
+      DXL_LOG_DEBUG("No controller active for joint '" << name << "'. Switching to position control mode.");
+      return dynamixel->writeControlMode(preferred_position_control_mode_, true);  // at startup torque is unknown
+    }
     return true;
   }
   // determine required control mode
@@ -214,6 +229,7 @@ std::string Joint::interfaceToLimitRegisterName(const std::string& interface_nam
 void Joint::resetGoalState(const std::string& interface_name)
 {
   double& value = getActuatorState().goal.at(interface_name);  // This should exist
+  ControlMode next_control_mode = getControlModeFromInterfaces(active_command_interfaces_);
 
   // Special handling for position
   if (interface_name == hardware_interface::HW_IF_POSITION) {
@@ -227,18 +243,22 @@ void Joint::resetGoalState(const std::string& interface_name)
                       "goal field. Add a position state interface to this joint to resolve this problem.");
       value = 0;
     }
-  } else {
-    if (std::find(active_command_interfaces_.begin(), active_command_interfaces_.end(), interface_name) !=
-            active_command_interfaces_.end() ||
-        active_command_interfaces_.empty()) {
-      // if no command interface is set (or known), reset to zero ( motor might be in velocity or current mode)
-      // also reset to zero if in velocity / effort mode
+  } else if (interface_name == hardware_interface::HW_IF_VELOCITY) {
+    if (control_mode_ == VELOCITY || next_control_mode == VELOCITY) {
       value = 0.0;
     } else {
-      // Default value, e.g. as velocity limit in position mode
-      value = default_goal_values_.at(interface_name);  // This should exist
+      value = default_goal_values_.at(hardware_interface::HW_IF_VELOCITY);
+    }
+  } else if (interface_name == hardware_interface::HW_IF_EFFORT || interface_name == hardware_interface::HW_IF_CURRENT) {
+    if (control_mode_ == CURRENT || next_control_mode == CURRENT) {
+      value = 0.0;  // controller must immediately write gravity compensating value !!
+    } else {
+      value = default_goal_values_.at(interface_name);
     }
   }
+  DXL_LOG_DEBUG("Resetting goal value of interface '" << interface_name << "' for joint '" << name << "' to " << value);
+  DXL_LOG_DEBUG("Active command interfaces for joint '" << name << "': " << iterableToString(active_command_interfaces_)
+                                                        << ", control mode: " << control_mode_);
   if (command_transmission) {
     command_transmission->actuator_to_joint();  // Unfortunately, there is no interface for single interface handles
   }
@@ -278,10 +298,10 @@ ControlMode Joint::getControlModeFromInterfaces(const std::vector<std::string>&
 
     return CURRENT;
   }
-  DXL_LOG_WARN("None out of the command interfaces "
-               << hardware_interface::HW_IF_POSITION << ", " << hardware_interface::HW_IF_VELOCITY << ", "
-               << hardware_interface::HW_IF_EFFORT << " have been requested. Defaulting to "
-               << hardware_interface::HW_IF_POSITION << " mode");
+  DXL_LOG_DEBUG("None out of the command interfaces "
+                << hardware_interface::HW_IF_POSITION << ", " << hardware_interface::HW_IF_VELOCITY << ", "
+                << hardware_interface::HW_IF_EFFORT << " have been requested. Defaulting to "
+                << hardware_interface::HW_IF_POSITION << " mode");
   return POSITION;
 }