Change logic from motor_power_enable to e_stop_torque_enable

husarion · Jan 10, 2025 · 7761069 · 7761069
1 parent 387071e
commit 7761069
Show file tree

Hide file tree

Showing 7 changed files with 15 additions and 9 deletions.
diff --git a/ROS_API.md b/ROS_API.md
@@ -118,9 +118,9 @@ Below is information about the physical robot API. For the simulation, topics an
 | ✅   | ❌   | `hardware/charger_enable`                         | Enables or disables external charger. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                          |
 | ✅   | ❌   | `hardware/digital_power_enable`                   | Enables or disables digital power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                             |
 | ✅   | ✅   | `hardware/e_stop_reset`                           | Resets E-stop. <br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                                 |
+| ✅   | ❌   | `hardware/e_stop_torque_enable`                     | Enables or disables motor power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                               |
 | ✅   | ✅   | `hardware/e_stop_trigger`                         | Triggers E-stop. <br/> [std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html)                                                                                               |
 | ✅   | ❌   | `hardware/fan_enable`                             | Enables or disables fan. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                                       |
-| ✅   | ❌   | `hardware/motor_power_enable`                     | Enables or disables motor power. <br/> [std_srvs/srv/SetBool](https://docs.ros2.org/latest/api/std_srvs/srv/SetBool.html)                                                                               |
 | ✅   | ✅   | `lights/set_animation`                            | Sets LED animation. <br/> [husarion_ugv_msgs/srv/SetLEDAnimation](husarion_ugv_msgs/srv/SetLEDAnimation.srv)                                                                                                  |
 | ✅   | ✅   | `localization/enable`                             | Enable EKF node. <br/> [std_srvs/srv/Empty](https://docs.ros2.org/latest/api/std_srvs/srv/Empty.html)                                                                                                   |
 | ✅   | ❌   | `lights/set_brightness`                           | Sets global LED brightness, value ranges from **0.0** to **1.0**. <br/> [husarion_ugv_msgs/SetLEDBrightness](husarion_ugv_msgs/srv/SetLEDBrightness.srv)                                                       |

diff --git a/husarion_ugv_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py
@@ -218,7 +218,7 @@ def generate_launch_description():
             ("hardware_controller/io_state", "hardware/io_state"),
             ("hardware_controller/led_control_enable", "hardware/led_control_enable"),
             ("hardware_controller/robot_driver_state", "hardware/robot_driver_state"),
-            ("hardware_controller/motor_power_enable", "hardware/motor_power_enable"),
+            ("hardware_controller/e_stop_torque_enable", "hardware/e_stop_torque_enable"),
             ("imu_broadcaster/imu", "imu/data"),
             ("imu_broadcaster/transition_event", "_imu_broadcaster/transition_event"),
             (

diff --git a/husarion_ugv_hardware_interfaces/README.md b/husarion_ugv_hardware_interfaces/README.md
@@ -26,7 +26,7 @@ Plugins for Panther and Lynx are based on an abstraction called UGVSystem. Most
 - `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop.
 - `hardware/fan_enable` [*std_srvs/SetBool*]: Enables or disables fan.
 - `hardware/led_control_enable` [*std_srvs/SetBool*]: Enables or disables SBC (Single Board Computer) control over the LEDs.
-- `hardware/motor_power_enable` [*std_srvs/SetBool*]: Enables or disables motor power.
+- `hardware/e_stop_torque_enable` [*std_srvs/SetBool*]: Enables or disables motor power.
 
 #### Parameters
 

diff --git a/..._hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp b/..._hardware_interfaces/include/husarion_ugv_hardware_interfaces/robot_system/ugv_system.hpp
@@ -110,7 +110,7 @@ class UGVSystem : public hardware_interface::SystemInterface
   void HandleRobotDriverWriteOperation(std::function<void()> write_operation);
   virtual std::vector<float> GetSpeedCommands() const = 0;
 
-  void MotorsPowerEnable(const bool enable);
+  void EStopTorqueEnable(const bool enable);
 
   virtual void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) = 0;
   virtual void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status) = 0;

diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/robot_driver/roboteq_data_converters.cpp
@@ -102,7 +102,7 @@ FaultFlag::FaultFlag()
     "overvoltage",
     "undervoltage",
     "short_circuit",
-    "emergency_stop",
+    "emergency_stop (torque disabled)",  // Roboteq E-stop differs from ROS E-stop
     "motor_or_sensor_setup_fault",
     "mosfet_failure",
     "default_config_loaded_at_startup",

diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/system_e_stop.cpp
@@ -55,6 +55,7 @@ void EStop::TriggerEStop()
 
 void EStop::ResetEStop()
 {
+  robot_driver_->TurnOffEStop();
   if (e_stop_manipulation_mtx_.try_lock()) {
     std::lock_guard<std::mutex> e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock);
 

diff --git a/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp b/husarion_ugv_hardware_interfaces/src/robot_system/ugv_system.cpp
@@ -129,7 +129,8 @@ CallbackReturn UGVSystem::on_activate(const rclcpp_lifecycle::State &)
     "~/led_control_enable",
     std::bind(&GPIOControllerInterface::LEDControlEnable, gpio_controller_, std::placeholders::_1));
   system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
-    "~/motor_power_enable", std::bind(&UGVSystem::MotorsPowerEnable, this, std::placeholders::_1));
+    "~/e_stop_torque_enable",
+    std::bind(&UGVSystem::EStopTorqueEnable, this, std::placeholders::_1));
 
   system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
     "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1,
@@ -525,8 +526,14 @@ bool UGVSystem::AreVelocityCommandsNearZero()
   return true;
 }
 
-void UGVSystem::MotorsPowerEnable(const bool enable)
+void UGVSystem::EStopTorqueEnable(const bool enable)
 {
+  const bool e_stop = e_stop_->ReadEStopState();
+  if (!e_stop) {
+    RCLCPP_WARN_STREAM(logger_, "Can't enable/disable torque when E-Stop is not triggered.");
+    return;
+  }
+
   try {
     {
       std::lock_guard<std::mutex> lck_g(*robot_driver_write_mtx_);
@@ -538,8 +545,6 @@ void UGVSystem::MotorsPowerEnable(const bool enable)
       }
     }
 
-    e_stop_->TriggerEStop();
-
     roboteq_error_filter_->SetClearErrorsFlag();
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
   } catch (const std::runtime_error & e) {