review fixes || remove manager

panther_hardware_interfaces/CMakeLists.txt

@@ -58,7 +58,7 @@ add_library(
   ${PROJECT_NAME} SHARED
   src/panther_system.cpp
   src/panther_system_ros_interface.cpp
-  src/panther_system_e_stop_manager.cpp
+  src/panther_system_e_stop.cpp
   src/motors_controller.cpp
   src/canopen_controller.cpp
   src/gpio_controller.cpp

panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp

@@ -115,13 +115,7 @@ class MotorsController
    * @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();
-
-  /**
-   * @brief Check if last velocity commands are near zero.
-   * @return Returns true if the last velocity commands are near zero, otherwise returns false.
-   */
-  bool AreVelocityCommandsNearZero();
+  inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); };
 
 private:
   void SetMotorsStates(
@@ -130,7 +124,6 @@ class MotorsController
     RoboteqData & data, const RoboteqDriverState & state, const timespec & current_time);
 
   bool initialized_ = false;
-  bool last_command_zero_ = false;
 
   CANopenController canopen_controller_;
 

panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp

@@ -32,7 +32,7 @@
 
 #include "panther_hardware_interfaces/gpio_controller.hpp"
 #include "panther_hardware_interfaces/motors_controller.hpp"
-#include "panther_hardware_interfaces/panther_system_e_stop_manager.hpp"
+#include "panther_hardware_interfaces/panther_system_e_stop.hpp"
 #include "panther_hardware_interfaces/panther_system_ros_interface.hpp"
 #include "panther_hardware_interfaces/roboteq_error_filter.hpp"
 
@@ -84,6 +84,7 @@ class PantherSystem : public hardware_interface::SystemInterface
 
   void UpdateHwStates();
   void UpdateMotorsStatesDataTimedOut();
+  bool AreVelocityCommandsNearZero();
 
   void UpdateDriverStateMsg();
   void UpdateFlagErrors();
@@ -116,7 +117,7 @@ class PantherSystem : public hardware_interface::SystemInterface
 
   std::shared_ptr<GPIOControllerInterface> gpio_controller_;
   std::shared_ptr<MotorsController> motors_controller_;
-  std::shared_ptr<EStopManager> e_stop_manager_;
+  std::shared_ptr<EStopStrategy> e_stop_strategy_;
 
   DrivetrainSettings drivetrain_settings_;
   CANopenSettings canopen_settings_;

panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_e_stop_manager.hpp → panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_e_stop.hpp

@@ -26,18 +26,6 @@
 namespace panther_hardware_interfaces
 {
 
-/**
- * @struct EStopManagerResources
- * @brief Holds all shared resources necessary for the emergency stop strategies.
- */
-struct EStopManagerResources
-{
-  std::shared_ptr<GPIOControllerInterface> gpio_controller;
-  std::shared_ptr<MotorsController> motors_controller;
-  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter;
-  std::shared_ptr<std::mutex> motor_controller_write_mtx;
-};
-
 /**
  * @class EStopStrategy
  * @brief Abstract base class defining the interface for emergency stop strategies.
@@ -51,22 +39,30 @@ class EStopStrategy
   virtual void TriggerEStop() = 0;
   virtual void ResetEStop() = 0;
 
-  /**
-   * @brief Sets manager resources to be used by the E-stop strategies.
-   * @param resources Shared resources necessary for managing E-stop.
-   */
-  void SetManagerResources(std::shared_ptr<EStopManagerResources> resources);
+  void SetResources(
+    std::shared_ptr<GPIOControllerInterface> gpio_controller,
+    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
+    std::shared_ptr<MotorsController> motors_controller,
+    std::shared_ptr<std::mutex> motor_controller_write_mtx,
+    std::function<bool()> zero_velocity_check)
+  {
+    gpio_controller_ = gpio_controller;
+    roboteq_error_filter_ = roboteq_error_filter;
+    motors_controller_ = motors_controller;
+    motor_controller_write_mtx_ = motor_controller_write_mtx;
+    ZeroVelocityCheck = zero_velocity_check;
+  }
 
 protected:
-  /**
-   * @brief Confirms that the emergency stop reset has been successful.
-   * @return `true` if reset was successful, `false` if emergency stop is still triggered.
-   */
-  bool ConfirmEStopResetSuccessful();
+  std::function<bool()> ZeroVelocityCheck;
+
+  std::shared_ptr<GPIOControllerInterface> gpio_controller_;
+  std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter_;
+  std::shared_ptr<MotorsController> motors_controller_;
+  std::shared_ptr<std::mutex> motor_controller_write_mtx_;
 
-  std::shared_ptr<EStopManagerResources> manager_resources_;
   std::mutex e_stop_manipulation_mtx_;
-  std::atomic_bool e_stop_triggered_;
+  std::atomic_bool e_stop_triggered_ = true;
 };
 
 /**
@@ -178,48 +174,6 @@ class EStopStrategyPTH10X : public EStopStrategy
   void ResetEStop() override;
 };
 
-/**
- * @class EStopManager
- * @brief Manages the emergency stop strategies and the transition between them.
- */
-class EStopManager
-{
-public:
-  EStopManager(
-    std::shared_ptr<GPIOControllerInterface> gpio_controller,
-    std::shared_ptr<MotorsController> motors_controller,
-    std::shared_ptr<RoboteqErrorFilter> roboteq_error_filter,
-    std::shared_ptr<std::mutex> motor_controller_write_mtx)
-  : resources_(std::make_shared<EStopManagerResources>(EStopManagerResources{
-      gpio_controller, motors_controller, roboteq_error_filter, motor_controller_write_mtx})){};
-
-  /**
-   * @brief Sets the strategy to be used for emergency stopping.
-   * @param strategy The unique pointer to the E-stop strategy to be employed.
-   */
-  void SetStrategy(std::unique_ptr<EStopStrategy> && strategy);
-
-  /**
-   * @brief Triggers an emergency stop in the current strategy.
-   */
-  void TriggerEStop();
-
-  /**
-   * @brief Resets the emergency stop in the current strategy.
-   */
-  void ResetEStop();
-
-  /**
-   * @brief Reads the current emergency stop state using the current strategy.
-   * @return `true` if E-stop is currently triggered, `false` otherwise.
-   */
-  bool ReadEStopState();
-
-private:
-  std::shared_ptr<EStopManagerResources> resources_;
-  std::unique_ptr<EStopStrategy> strategy_;
-};
-
 }  // namespace panther_hardware_interfaces
 
 #endif  // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_E_STOP_MANAGER_HPP_

panther_hardware_interfaces/src/motors_controller.cpp

@@ -135,11 +135,6 @@ void MotorsController::UpdateDriversState()
 void MotorsController::SendSpeedCommands(
   const float speed_fl, const float speed_fr, const float speed_rl, const float speed_rr)
 {
-  const float zero_threshold = std::numeric_limits<float>::epsilon();
-  last_command_zero_ = std::abs(speed_fl) <= zero_threshold &&
-                       std::abs(speed_fr) <= zero_threshold &&
-                       std::abs(speed_rl) <= zero_threshold && std::abs(speed_rr) <= zero_threshold;
-
   // Channel 1 - right motor, Channel 2 - left motor
   try {
     canopen_controller_.GetFrontDriver()->SendRoboteqCmd(
@@ -214,13 +209,6 @@ void MotorsController::TurnOnSafetyStop()
   }
 }
 
-void MotorsController::AttemptErrorFlagResetWithZeroSpeed()
-{
-  SendSpeedCommands(0.0, 0.0, 0.0, 0.0);
-}
-
-bool MotorsController::AreVelocityCommandsNearZero() { return last_command_zero_; }
-
 void MotorsController::SetMotorsStates(
   RoboteqData & data, const RoboteqMotorsStates & states, const timespec & current_time)
 {

panther_hardware_interfaces/src/panther_system.cpp

@@ -72,15 +72,12 @@ CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &)
   RCLCPP_INFO_STREAM(
     logger_, "Creating system entities for the Panther version: " << panther_version_);
 
-  e_stop_manager_ = std::make_shared<EStopManager>(
-    gpio_controller_, motors_controller_, roboteq_error_filter_, motor_controller_write_mtx_);
-
   if (panther_version_ >= 1.2 - std::numeric_limits<float>::epsilon()) {
     gpio_controller_ = std::make_shared<GPIOControllerPTH12X>();
-    e_stop_manager_->SetStrategy(std::make_unique<EStopStrategyPTH12X>());
+    e_stop_strategy_ = std::make_shared<EStopStrategyPTH12X>();
   } else {
     gpio_controller_ = std::make_shared<GPIOControllerPTH10X>();
-    e_stop_manager_->SetStrategy(std::make_unique<EStopStrategyPTH10X>());
+    e_stop_strategy_ = std::make_shared<EStopStrategyPTH10X>();
   }
 
   try {
@@ -90,6 +87,7 @@ CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &)
     return CallbackReturn::ERROR;
   }
 
+  motor_controller_write_mtx_ = std::make_shared<std::mutex>();
   motors_controller_ = std::make_shared<MotorsController>(canopen_settings_, drivetrain_settings_);
 
   if (!OperationWithAttempts(
@@ -100,6 +98,10 @@ CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &)
     return CallbackReturn::ERROR;
   }
 
+  e_stop_strategy_->SetResources(
+    gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_,
+    std::bind(&PantherSystem::AreVelocityCommandsNearZero, this));
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -152,10 +154,10 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &)
     std::bind(&PantherSystem::MotorsPowerEnable, this, std::placeholders::_1));
 
   panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
-    "~/e_stop_trigger", std::bind(&EStopManager::TriggerEStop, e_stop_manager_), 1,
+    "~/e_stop_trigger", std::bind(&EStopStrategy::TriggerEStop, e_stop_strategy_), 1,
     rclcpp::CallbackGroupType::MutuallyExclusive);
   panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
-    "~/e_stop_reset", std::bind(&EStopManager::ResetEStop, e_stop_manager_), 2,
+    "~/e_stop_reset", std::bind(&EStopStrategy::ResetEStop, e_stop_strategy_), 2,
     rclcpp::CallbackGroupType::MutuallyExclusive);
 
   panther_system_ros_interface_->AddDiagnosticTask(
@@ -170,7 +172,7 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &)
   const auto io_state = gpio_controller_->QueryControlInterfaceIOStates();
   panther_system_ros_interface_->InitializeAndPublishIOStateMsg(io_state);
 
-  panther_system_ros_interface_->PublishEStopStateMsg(e_stop_manager_->ReadEStopState());
+  panther_system_ros_interface_->PublishEStopStateMsg(e_stop_strategy_->ReadEStopState());
 
   return CallbackReturn::SUCCESS;
 }
@@ -180,7 +182,7 @@ CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State &)
   RCLCPP_INFO(logger_, "Deactivating Panther System");
 
   try {
-    e_stop_manager_->TriggerEStop();
+    e_stop_strategy_->TriggerEStop();
   } catch (const std::runtime_error & e) {
     RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what());
     return CallbackReturn::ERROR;
@@ -195,7 +197,7 @@ CallbackReturn PantherSystem::on_shutdown(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(logger_, "Shutting down Panther System");
   try {
-    e_stop_manager_->TriggerEStop();
+    e_stop_strategy_->TriggerEStop();
   } catch (const std::runtime_error & e) {
     RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what());
     return CallbackReturn::ERROR;
@@ -216,7 +218,7 @@ CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &)
   RCLCPP_INFO(logger_, "Handling Panther System error");
 
   try {
-    e_stop_manager_->TriggerEStop();
+    e_stop_strategy_->TriggerEStop();
   } catch (const std::runtime_error & e) {
     RCLCPP_ERROR_STREAM(logger_, "Setting E-stop failed: " << e.what());
     return CallbackReturn::ERROR;
@@ -266,7 +268,7 @@ return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duratio
 {
   UpdateMotorsStates();
 
-  const bool e_stop = e_stop_manager_->ReadEStopState();
+  const bool e_stop = e_stop_strategy_->ReadEStopState();
   panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop);
 
   if (time >= next_driver_state_update_time_) {
@@ -281,7 +283,7 @@ return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duratio
 return_type PantherSystem::write(
   const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
 {
-  const bool e_stop = e_stop_manager_->ReadEStopState();
+  const bool e_stop = e_stop_strategy_->ReadEStopState();
 
   if (!e_stop) {
     HandlePDOWriteOperation([this] {
@@ -597,6 +599,16 @@ void PantherSystem::HandlePDOWriteOperation(std::function<void()> pdo_write_oper
   }
 }
 
+bool PantherSystem::AreVelocityCommandsNearZero()
+{
+  for (const auto & cmd : hw_commands_velocities_) {
+    if (std::abs(cmd) > std::numeric_limits<double>::epsilon()) {
+      return false;
+    }
+  }
+  return true;
+}
+
 void PantherSystem::MotorsPowerEnable(const bool enable)
 {
   try {
@@ -610,7 +622,7 @@ void PantherSystem::MotorsPowerEnable(const bool enable)
       }
     }
 
-    e_stop_manager_->TriggerEStop();
+    e_stop_strategy_->TriggerEStop();
 
     roboteq_error_filter_->SetClearErrorsFlag();
     roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false);