Merge pull request #258 from husarion/ros2-control-fix-err-flag-reset

ROS 2- Fix Error Clearing Mechanism for Roboteq Controllers

panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp

@@ -111,6 +111,12 @@ class MotorsController
    */
   void TurnOnSafetyStop();
 
+  /**
+   * @brief Attempt to clear driver error flags by sending 0 velocity commands to motors. If Roboteq
+   * driver faults still exist, the error flag will remain active.
+   */
+  void AttemptErrorFlagResetWithZeroSpeed();
+
 private:
   void SetMotorsStates(
     RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time);

panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp

@@ -88,9 +88,7 @@ class PantherSystem : public hardware_interface::SystemInterface
   void UpdateFlagErrors();
   void UpdateDriverStateDataTimedOut();
 
-  void SendCommands();
-  void CheckErrorsAndSetEStop();
-  bool CheckIfSafetyStopActive();
+  void HandlePDOWriteOperation(std::function<void()> pdo_write_operation);
   bool AreVelocityCommandsNearZero();
 
   void MotorsPowerEnable(const bool enable);

panther_hardware_interfaces/src/motors_controller.cpp

@@ -236,4 +236,9 @@ void MotorsController::SetDriverState(
   data.SetDriverState(state, data_timed_out);
 }
 
+void MotorsController::AttemptErrorFlagResetWithZeroSpeed()
+{
+  SendSpeedCommands(0.0, 0.0, 0.0, 0.0);
+}
+
 }  // namespace panther_hardware_interfaces

panther_hardware_interfaces/src/panther_system.cpp

@@ -75,14 +75,34 @@ CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &)
     gpio_controller_ = std::make_shared<GPIOControllerPTH12X>();
     use_can_for_e_stop_trigger_ = false;
 
+    // TODO: @pkowalsk1 move estop logic to separate abstraction
     ReadEStop = [this]() {
-      return !gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::E_STOP_RESET);
+      const bool e_stop_triggered =
+        !gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::E_STOP_RESET);
+
+      // In the case where E-Stop is triggered by another device within the robot's system (e.g.,
+      // Roboteq or Safety Board), disabling the software Watchdog is necessary to prevent an
+      // uncontrolled reset.
+      if (e_stop_triggered) {
+        gpio_controller_->EStopTrigger();
+      }
+
+      return e_stop_triggered;
     };
   } else {
     gpio_controller_ = std::make_shared<GPIOControllerPTH10X>();
     use_can_for_e_stop_trigger_ = true;
 
-    ReadEStop = [this]() { return e_stop_.load(); };
+    ReadEStop = [this]() {
+      const bool motors_on = gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT);
+      const bool driver_error = roboteq_error_filter_->IsError();
+
+      if ((driver_error || !motors_on) && !e_stop_.load()) {
+        SetEStop();
+      }
+
+      return e_stop_.load();
+    };
   }
 
   try {
@@ -286,17 +306,12 @@ return_type PantherSystem::write(
 {
   last_commands_zero_ = AreVelocityCommandsNearZero();
 
-  try {
-    CheckErrorsAndSetEStop();
-  } catch (const std::runtime_error & e) {
-    RCLCPP_FATAL_STREAM(logger_, "Error when handling E-stop: " << e.what());
-    return return_type::ERROR;
-  }
-
-  // "soft" error - still there is communication over CAN with drivers, so publishing feedback is
-  // continued, only commands are ignored
   if (!e_stop_) {
-    SendCommands();
+    HandlePDOWriteOperation([this] {
+      motors_controller_->SendSpeedCommands(
+        hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2],
+        hw_commands_velocities_[3]);
+    });
   }
 
   return return_type::OK;
@@ -563,6 +578,8 @@ void PantherSystem::UpdateFlagErrors()
         << "\tFront: " << motors_controller_->GetFrontData().GetFlagErrorLog()
         << "\tRear: " << motors_controller_->GetRearData().GetFlagErrorLog());
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, true);
+
+    HandlePDOWriteOperation([this] { motors_controller_->AttemptErrorFlagResetWithZeroSpeed(); });
   } else {
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);
   }
@@ -583,7 +600,7 @@ void PantherSystem::UpdateDriverStateDataTimedOut()
   }
 }
 
-void PantherSystem::SendCommands()
+void PantherSystem::HandlePDOWriteOperation(std::function<void()> pdo_write_operation)
 {
   try {
     {
@@ -593,10 +610,7 @@ void PantherSystem::SendCommands()
         throw std::runtime_error(
           "Can't acquire mutex for writing commands - E-stop is being triggered");
       }
-
-      motors_controller_->SendSpeedCommands(
-        hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2],
-        hw_commands_velocities_[3]);
+      pdo_write_operation();
     }
 
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::WRITE_PDO_CMDS, false);
@@ -606,33 +620,6 @@ void PantherSystem::SendCommands()
   }
 }
 
-void PantherSystem::CheckErrorsAndSetEStop()
-{
-  if (roboteq_error_filter_->IsError() && !e_stop_) {
-    if (!CheckIfSafetyStopActive()) {
-      RCLCPP_ERROR(
-        logger_,
-        "Error detected and at least on of the channels is not in the safety stop state, sending "
-        "E-stop request...");
-      try {
-        SetEStop();
-      } catch (const std::runtime_error & e) {
-        throw e;
-      }
-    }
-  }
-}
-
-bool PantherSystem::CheckIfSafetyStopActive()
-{
-  const auto & front_data = motors_controller_->GetFrontData();
-  const auto & rear_data = motors_controller_->GetRearData();
-  return front_data.GetLeftRuntimeError().GetMessage().safety_stop_active &&
-         front_data.GetRightRuntimeError().GetMessage().safety_stop_active &&
-         rear_data.GetLeftRuntimeError().GetMessage().safety_stop_active &&
-         rear_data.GetRightRuntimeError().GetMessage().safety_stop_active;
-}
-
 bool PantherSystem::AreVelocityCommandsNearZero()
 {
   for (const auto & cmd : hw_commands_velocities_) {

panther_manager/config/manager_bt_config_106.yaml

@@ -1,15 +1,18 @@
-launch_safety_tree: false
-launch_shutdown_tree: false
-battery_percent_window_len: 6
-lights:
-  critical_battery_anim_period: 15.0
-  critical_battery_threshold_percent: 0.1
-  battery_state_anim_period: 120.0
-  low_battery_anim_period: 30.0
-  low_battery_threshold_percent: 0.4
-  update_charging_anim_step: 0.1
-  timer_frequency: 10.0
-plugin_libs:
-  - tick_after_timeout_bt_node
-ros_plugin_libs:
-  - call_set_led_animation_service_bt_node
+/**:
+  manager_bt_node:
+    ros__parameters:
+      launch_safety_tree: false
+      launch_shutdown_tree: false
+      battery_percent_window_len: 6
+      lights:
+        critical_battery_anim_period: 15.0
+        critical_battery_threshold_percent: 0.1
+        battery_state_anim_period: 120.0
+        low_battery_anim_period: 30.0
+        low_battery_threshold_percent: 0.4
+        update_charging_anim_step: 0.1
+        timer_frequency: 10.0
+      plugin_libs:
+        - tick_after_timeout_bt_node
+      ros_plugin_libs:
+        - call_set_led_animation_service_bt_node