diff --git a/panther_hardware_interfaces/CMakeLists.txt b/panther_hardware_interfaces/CMakeLists.txt index 4b9acd6e9..bb4b6a8ee 100644 --- a/panther_hardware_interfaces/CMakeLists.txt +++ b/panther_hardware_interfaces/CMakeLists.txt @@ -58,6 +58,7 @@ add_library( ${PROJECT_NAME} SHARED src/panther_system.cpp src/panther_system_ros_interface.cpp + src/panther_system_e_stop.cpp src/motors_controller.cpp src/canopen_controller.cpp src/gpio_controller.cpp diff --git a/panther_hardware_interfaces/CODE_STRUCTURE.md b/panther_hardware_interfaces/CODE_STRUCTURE.md index 0d1bb33dd..1c18ea95f 100644 --- a/panther_hardware_interfaces/CODE_STRUCTURE.md +++ b/panther_hardware_interfaces/CODE_STRUCTURE.md @@ -50,6 +50,14 @@ The GPIOController provides wrappers for the GPIO driver from the `panther_gpiod * `GPIOControllerPTH10X`: Class with specific logic for the Panther robot with version below 1.20. * `Watchdog`: Entity responsible for spinning the software Watchdog. It periodically sets the high and low states of specific GPIO Watchdog pin. Used only with `GPIOControllerPTH12X`. +## EStop + +Implementation of emergency stop handling. + +* `EStopInterface`: Interface for versioned emergency stop implementations. +* `EStopPTH12X`: Class with specific logic for the Panther robot with version 1.20 and above. +* `EStopPTH10X`: Class with specific logic for the Panther robot with version below 1.20. + ## PantherSystemRosInterface A class that takes care of additional ROS interface of panther system, such as publishing driver state and providing service for clearing errors. diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp index 7e1bf11fc..4301caa22 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/motors_controller.hpp @@ -115,7 +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(); + inline void AttemptErrorFlagResetWithZeroSpeed() { SendSpeedCommands(0.0, 0.0, 0.0, 0.0); }; private: void SetMotorsStates( diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp index 91f0d48f7..8ffc2b70a 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system.hpp @@ -32,6 +32,7 @@ #include "panther_hardware_interfaces/gpio_controller.hpp" #include "panther_hardware_interfaces/motors_controller.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" @@ -78,23 +79,25 @@ class PantherSystem : public hardware_interface::SystemInterface void ReadParametersAndCreateRoboteqErrorFilter(); void ReadDriverStatesUpdateFrequency(); + void ConfigureGPIOController(); + void ConfigureMotorsController(); + void ConfigureEStop(); + void UpdateMotorsStates(); void UpdateDriverState(); void UpdateHwStates(); void UpdateMotorsStatesDataTimedOut(); + bool AreVelocityCommandsNearZero(); + bool IsPantherVersionAtLeast(const float version); void UpdateDriverStateMsg(); void UpdateFlagErrors(); void UpdateDriverStateDataTimedOut(); void HandlePDOWriteOperation(std::function pdo_write_operation); - bool AreVelocityCommandsNearZero(); void MotorsPowerEnable(const bool enable); - void SetEStop(); - void ResetEStop(); - std::function ReadEStop; void DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status); void DiagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper & status); @@ -119,6 +122,7 @@ class PantherSystem : public hardware_interface::SystemInterface std::shared_ptr gpio_controller_; std::shared_ptr motors_controller_; + std::shared_ptr e_stop_; DrivetrainSettings drivetrain_settings_; CANopenSettings canopen_settings_; @@ -142,11 +146,7 @@ class PantherSystem : public hardware_interface::SystemInterface float panther_version_; - std::atomic_bool e_stop_ = true; - std::atomic_bool use_can_for_e_stop_trigger_ = false; - std::atomic_bool last_commands_zero_ = false; - std::mutex e_stop_manipulation_mtx_; - std::mutex motor_controller_write_mtx_; + std::shared_ptr motor_controller_write_mtx_; rclcpp::Time next_driver_state_update_time_{0, 0, RCL_ROS_TIME}; rclcpp::Duration driver_states_update_period_{0, 0}; diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_e_stop.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_e_stop.hpp new file mode 100644 index 000000000..68f0c4086 --- /dev/null +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_e_stop.hpp @@ -0,0 +1,191 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_E_STOP_MANAGER_HPP_ +#define PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_E_STOP_MANAGER_HPP_ + +#include +#include +#include + +#include "panther_hardware_interfaces/gpio_controller.hpp" +#include "panther_hardware_interfaces/motors_controller.hpp" +#include "panther_hardware_interfaces/roboteq_error_filter.hpp" + +namespace panther_hardware_interfaces +{ + +/** + * @class EStopInterface + * @brief Abstract base class defining the interface for emergency stop detailed implementations. + */ +class EStopInterface +{ +public: + EStopInterface( + std::shared_ptr gpio_controller, + std::shared_ptr roboteq_error_filter, + std::shared_ptr motors_controller, + std::shared_ptr motor_controller_write_mtx, + std::function 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){}; + + virtual ~EStopInterface() = default; + + virtual bool ReadEStopState() = 0; + virtual void TriggerEStop() = 0; + virtual void ResetEStop() = 0; + +protected: + std::shared_ptr gpio_controller_; + std::shared_ptr roboteq_error_filter_; + std::shared_ptr motors_controller_; + std::shared_ptr motor_controller_write_mtx_; + + std::function ZeroVelocityCheck; + + std::mutex e_stop_manipulation_mtx_; + std::atomic_bool e_stop_triggered_ = true; +}; + +/** + * @class EStopPTH12X + * @brief Implements the emergency stop for the PTH12X hardware variant. + */ +class EStopPTH12X : public EStopInterface +{ +public: + EStopPTH12X( + std::shared_ptr gpio_controller, + std::shared_ptr roboteq_error_filter, + std::shared_ptr motors_controller, + std::shared_ptr motor_controller_write_mtx, + std::function zero_velocity_check) + : EStopInterface( + gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, + zero_velocity_check){}; + + virtual ~EStopPTH12X() override = default; + + /** + * @brief Checks the emergency stop state. + * + * 1. Check if ESTOP GPIO pin is not active. If is not it means that E-Stop is triggered by + * another device within the robot's system (e.g., Roboteq controller or Safety Board), + * disabling the software Watchdog is necessary to prevent an uncontrolled reset. + * 2. If there is a need, disable software Watchdog using + * GPIOControllerInterface::EStopTrigger method. + * 3. Return ESTOP GPIO pin state. + */ + bool ReadEStopState() override; + + /** + * @brief Triggers the emergency stop. + * + * 1. Interrupt the E-Stop resetting process if it is in progress. + * 2. Attempt to trigger the E-Stop using GPIO by disabling the software-controlled watchdog. + * 3. If successful, set e_stop_triggered_ to true; otherwise, throw a std::runtime_error + * exception. + * + * @throws std::runtime_error if triggering the E-stop using GPIO fails. + */ + void TriggerEStop() override; + + /** + * @brief Resets the emergency stop. + * + * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement + * after an E-stop state change. + * 2. Attempt to reset the E-Stop using GPIO by manipulating the ESTOP GPIO pin. This operation + * may take some time and can be interrupted by the E-Stop trigger process. + * 3. Set the clear_error flag to allow for clearing of Roboteq errors. + * 4. Confirm the E-Stop reset was successful with the ReadEStopState method. + * + * @throws EStopResetInterrupted if the E-stop reset operation was halted because the E-stop was + * triggered again. + * @throws std::runtime_error if an error occurs when trying to reset the E-stop using GPIO. + */ + void ResetEStop() override; +}; + +/** + * @class EStopPTH10X + * @brief Implements the emergency stop for the PTH10X hardware variant. In this robot + * version, only a software-based E-Stop is supported. There are no hardware components that + * implement E-Stop functionality. + */ +class EStopPTH10X : public EStopInterface +{ +public: + EStopPTH10X( + std::shared_ptr gpio_controller, + std::shared_ptr roboteq_error_filter, + std::shared_ptr motors_controller, + std::shared_ptr motor_controller_write_mtx, + std::function zero_velocity_check) + : EStopInterface( + gpio_controller, roboteq_error_filter, motors_controller, motor_controller_write_mtx, + zero_velocity_check){}; + + virtual ~EStopPTH10X() override = default; + + /** + * @brief Checks the emergency stop state. + * + * 1. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered + * up. + * 2. Check for any errors reported by the Roboteq controller. + * 3. If the E-Stop is not currently triggered, initiate an E-Stop if the motors are not powered + * up or if a driver error has occurred. + * 4. Return the actual state of the E-Stop. + * + * @return A boolean indicating whether the E-Stop is currently triggered `true` or not `false`. + */ + bool ReadEStopState() override; + + /** + * @brief Trigger the emergency stop state. + * + * 1. Send a command to the Roboteq controllers to enable the Safety Stop. + * Note: The Safety Stop is a specific state of the Roboteq controller, distinct from + * the E-Stop state of the Panther robot. + * 2. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. + */ + void TriggerEStop() override; + + /** + * @brief Resets the emergency stop. + * + * 1. Verify that the last velocity commands are zero to prevent an uncontrolled robot movement + * after an E-stop state change. + * 2. Verify if the main switch is in the STAGE2 position to confirm if the motors are powered + * up. + * 3. Check for any errors reported by the Roboteq controller. + * 4. Set the clear_error flag to allow for clearing of Roboteq errors. + * 5. Confirm the E-Stop reset was successful with the ReadEStopState method. + * 6. Update the e_stop_triggered_ flag to indicate that the E-Stop state has been triggered. + * + * @throws std::runtime_error if the E-stop reset operation was halted because the E-stop was + * triggered again, motors are not powered or motor controller is in an error state. + */ + void ResetEStop() override; +}; + +} // namespace panther_hardware_interfaces + +#endif // PANTHER_HARDWARE_INTERFACES_PANTHER_SYSTEM_E_STOP_MANAGER_HPP_ diff --git a/panther_hardware_interfaces/src/motors_controller.cpp b/panther_hardware_interfaces/src/motors_controller.cpp index b2d18bc85..b3ce589b9 100644 --- a/panther_hardware_interfaces/src/motors_controller.cpp +++ b/panther_hardware_interfaces/src/motors_controller.cpp @@ -236,9 +236,4 @@ 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 diff --git a/panther_hardware_interfaces/src/panther_system.cpp b/panther_hardware_interfaces/src/panther_system.cpp index 07e2c326a..b97adb539 100644 --- a/panther_hardware_interfaces/src/panther_system.cpp +++ b/panther_hardware_interfaces/src/panther_system.cpp @@ -69,56 +69,15 @@ CallbackReturn PantherSystem::on_init(const hardware_interface::HardwareInfo & h CallbackReturn PantherSystem::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(logger_, "Configuring Panther System"); - - RCLCPP_INFO_STREAM(logger_, "Creating GPIO controller for Panther version: " << panther_version_); - if (panther_version_ >= 1.2 - std::numeric_limits::epsilon()) { - gpio_controller_ = std::make_shared(); - use_can_for_e_stop_trigger_ = false; - - // TODO: @pkowalsk1 move estop logic to separate abstraction - ReadEStop = [this]() { - 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(); - use_can_for_e_stop_trigger_ = true; - - 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(); - }; - } + RCLCPP_INFO_STREAM( + logger_, "Creating system entities for the Panther version: " << panther_version_); try { - gpio_controller_->Start(); + ConfigureGPIOController(); + ConfigureMotorsController(); + ConfigureEStop(); } catch (const std::runtime_error & e) { - RCLCPP_FATAL_STREAM(logger_, "GPIO controller initialization failed. Error: " << e.what()); - return CallbackReturn::ERROR; - } - - motors_controller_ = std::make_shared(canopen_settings_, drivetrain_settings_); - - if (!OperationWithAttempts( - std::bind(&MotorsController::Initialize, motors_controller_), - max_roboteq_initialization_attempts_, - std::bind(&MotorsController::Deinitialize, motors_controller_))) { - RCLCPP_FATAL_STREAM(logger_, "Roboteq drivers initialization failed"); + RCLCPP_FATAL_STREAM(logger_, "System entities initialization failed. Error: " << e.what()); return CallbackReturn::ERROR; } @@ -133,6 +92,7 @@ CallbackReturn PantherSystem::on_cleanup(const rclcpp_lifecycle::State &) motors_controller_.reset(); gpio_controller_.reset(); + e_stop_.reset(); return CallbackReturn::SUCCESS; } @@ -174,10 +134,10 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) std::bind(&PantherSystem::MotorsPowerEnable, this, std::placeholders::_1)); panther_system_ros_interface_->AddService>( - "~/e_stop_trigger", std::bind(&PantherSystem::SetEStop, this), 1, + "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, rclcpp::CallbackGroupType::MutuallyExclusive); panther_system_ros_interface_->AddService>( - "~/e_stop_reset", std::bind(&PantherSystem::ResetEStop, this), 2, + "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, rclcpp::CallbackGroupType::MutuallyExclusive); panther_system_ros_interface_->AddDiagnosticTask( @@ -192,8 +152,7 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) const auto io_state = gpio_controller_->QueryControlInterfaceIOStates(); panther_system_ros_interface_->InitializeAndPublishIOStateMsg(io_state); - e_stop_ = ReadEStop(); - panther_system_ros_interface_->PublishEStopStateMsg(e_stop_); + panther_system_ros_interface_->PublishEStopStateMsg(e_stop_->ReadEStopState()); return CallbackReturn::SUCCESS; } @@ -203,7 +162,7 @@ CallbackReturn PantherSystem::on_deactivate(const rclcpp_lifecycle::State &) RCLCPP_INFO(logger_, "Deactivating Panther System"); try { - SetEStop(); + e_stop_->TriggerEStop(); } catch (const std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger_, "Deactivation failed: " << e.what()); return CallbackReturn::ERROR; @@ -218,18 +177,19 @@ CallbackReturn PantherSystem::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(logger_, "Shutting down Panther System"); try { - SetEStop(); + e_stop_->TriggerEStop(); } catch (const std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger_, "Shutdown failed: " << e.what()); return CallbackReturn::ERROR; } - gpio_controller_.reset(); - motors_controller_->Deinitialize(); motors_controller_.reset(); + gpio_controller_.reset(); + panther_system_ros_interface_.reset(); + e_stop_.reset(); return CallbackReturn::SUCCESS; } @@ -239,7 +199,7 @@ CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &) RCLCPP_INFO(logger_, "Handling Panther System error"); try { - SetEStop(); + e_stop_->TriggerEStop(); } catch (const std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger_, "Setting E-stop failed: " << e.what()); return CallbackReturn::ERROR; @@ -255,6 +215,7 @@ CallbackReturn PantherSystem::on_error(const rclcpp_lifecycle::State &) motors_controller_.reset(); gpio_controller_.reset(); + e_stop_.reset(); return CallbackReturn::SUCCESS; } @@ -289,8 +250,8 @@ return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duratio { UpdateMotorsStates(); - e_stop_ = ReadEStop(); - panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop_); + const bool e_stop = e_stop_->ReadEStopState(); + panther_system_ros_interface_->PublishEStopStateIfChanged(e_stop); if (time >= next_driver_state_update_time_) { UpdateDriverState(); @@ -304,9 +265,9 @@ return_type PantherSystem::read(const rclcpp::Time & time, const rclcpp::Duratio return_type PantherSystem::write( const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) { - last_commands_zero_ = AreVelocityCommandsNearZero(); + const bool e_stop = e_stop_->ReadEStopState(); - if (!e_stop_) { + if (!e_stop) { HandlePDOWriteOperation([this] { motors_controller_->SendSpeedCommands( hw_commands_velocities_[0], hw_commands_velocities_[1], hw_commands_velocities_[2], @@ -468,6 +429,49 @@ void PantherSystem::ReadDriverStatesUpdateFrequency() rclcpp::Duration::from_seconds(1.0f / driver_states_update_frequency); } +void PantherSystem::ConfigureGPIOController() +{ + if (IsPantherVersionAtLeast(1.2)) { + gpio_controller_ = std::make_shared(); + } else { + gpio_controller_ = std::make_shared(); + } + + gpio_controller_->Start(); +} + +void PantherSystem::ConfigureMotorsController() +{ + motor_controller_write_mtx_ = std::make_shared(); + motors_controller_ = std::make_shared(canopen_settings_, drivetrain_settings_); + + if (!OperationWithAttempts( + std::bind(&MotorsController::Initialize, motors_controller_), + max_roboteq_initialization_attempts_, + std::bind(&MotorsController::Deinitialize, motors_controller_))) { + throw std::runtime_error("Roboteq drivers initialization failed"); + } +} + +void PantherSystem::ConfigureEStop() +{ + if ( + !gpio_controller_ || !roboteq_error_filter_ || !motors_controller_ || + !motor_controller_write_mtx_) { + throw std::runtime_error("Setup entities first."); + } + + if (IsPantherVersionAtLeast(1.2f)) { + e_stop_ = std::make_shared( + gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, + std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); + } else { + e_stop_ = std::make_shared( + gpio_controller_, roboteq_error_filter_, motors_controller_, motor_controller_write_mtx_, + std::bind(&PantherSystem::AreVelocityCommandsNearZero, this)); + } +} + void PantherSystem::UpdateMotorsStates() { try { @@ -605,7 +609,7 @@ void PantherSystem::HandlePDOWriteOperation(std::function pdo_write_oper try { { std::unique_lock motor_controller_write_lck( - motor_controller_write_mtx_, std::defer_lock); + *motor_controller_write_mtx_, std::defer_lock); if (!motor_controller_write_lck.try_lock()) { throw std::runtime_error( "Can't acquire mutex for writing commands - E-stop is being triggered"); @@ -630,11 +634,16 @@ bool PantherSystem::AreVelocityCommandsNearZero() return true; } +bool PantherSystem::IsPantherVersionAtLeast(const float version) +{ + return panther_version_ >= version - std::numeric_limits::epsilon(); +} + void PantherSystem::MotorsPowerEnable(const bool enable) { try { { - std::lock_guard lck_g(motor_controller_write_mtx_); + std::lock_guard lck_g(*motor_controller_write_mtx_); if (!enable) { motors_controller_->TurnOnEStop(); @@ -643,7 +652,7 @@ void PantherSystem::MotorsPowerEnable(const bool enable) } } - SetEStop(); + e_stop_->TriggerEStop(); roboteq_error_filter_->SetClearErrorsFlag(); roboteq_error_filter_->UpdateError(ErrorsFilterIds::ROBOTEQ_DRIVER, false); @@ -652,68 +661,6 @@ void PantherSystem::MotorsPowerEnable(const bool enable) } } -void PantherSystem::SetEStop() -{ - gpio_controller_->InterruptEStopReset(); - - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_); - - RCLCPP_INFO(logger_, "Setting E-stop"); - - try { - gpio_controller_->EStopTrigger(); - } catch (const std::runtime_error & e) { - RCLCPP_INFO_STREAM(logger_, "Trying to set E-stop using GPIO: " << e.what()); - - if (!use_can_for_e_stop_trigger_) { - throw std::runtime_error("Setting E-stop failed"); - } - - std::lock_guard lck_g(motor_controller_write_mtx_); - - try { - motors_controller_->TurnOnSafetyStop(); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger_, "Error when trying to set safety stop using CAN command: " << e.what()); - throw std::runtime_error("Both attempts at setting E-stop failed"); - } - } - - e_stop_ = true; -} - -void PantherSystem::ResetEStop() -{ - if (e_stop_manipulation_mtx_.try_lock()) { - std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); - - RCLCPP_INFO(logger_, "Resetting E-stop"); - - // On the side of the motors controller safety stop is reset by sending 0.0 commands - if (!last_commands_zero_) { - throw std::runtime_error( - "Can't reset E-stop - last velocity commands are different than zero. Make sure that your " - "controller sends zero commands before trying to reset E-stop."); - } - - try { - gpio_controller_->EStopReset(); - } catch (const EStopResetInterrupted & e) { - RCLCPP_INFO(logger_, "E-stop reset has been interrupted"); - return; - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Error when trying to reset E-stop using GPIO: " + std::string(e.what())); - } - - roboteq_error_filter_->SetClearErrorsFlag(); - e_stop_ = false; - } else { - RCLCPP_INFO(logger_, "E-stop trigger operation is in progress, skipping reset."); - } -} - void PantherSystem::DiagnoseErrors(diagnostic_updater::DiagnosticStatusWrapper & status) { unsigned char level{diagnostic_updater::DiagnosticStatusWrapper::OK}; diff --git a/panther_hardware_interfaces/src/panther_system_e_stop.cpp b/panther_hardware_interfaces/src/panther_system_e_stop.cpp new file mode 100644 index 000000000..92e5ccc17 --- /dev/null +++ b/panther_hardware_interfaces/src/panther_system_e_stop.cpp @@ -0,0 +1,137 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "panther_hardware_interfaces/panther_system_e_stop.hpp" + +namespace panther_hardware_interfaces +{ + +bool EStopPTH12X::ReadEStopState() +{ + 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_; +} + +void EStopPTH12X::TriggerEStop() +{ + gpio_controller_->InterruptEStopReset(); + + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_); + try { + gpio_controller_->EStopTrigger(); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Setting E-stop failed: " + std::string(e.what())); + } + + e_stop_triggered_ = true; +} + +void EStopPTH12X::ResetEStop() +{ + if (e_stop_manipulation_mtx_.try_lock()) { + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); + + if (!ZeroVelocityCheck()) { + throw std::runtime_error( + "Cannot reset the E-stop: the last velocity commands are not zero. Ensure that the " + "controller is sending zero velocity commands or not sending any velocity commands before " + "attempting to reset the E-stop."); + } + + try { + gpio_controller_->EStopReset(); + } catch (const EStopResetInterrupted & e) { + throw std::runtime_error( + "The E-stop reset operation was halted because the E-stop was triggered again."); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Error when trying to reset E-stop using GPIO: " + std::string(e.what())); + } + + roboteq_error_filter_->SetClearErrorsFlag(); + + e_stop_triggered_ = false; + } else { + std::fprintf(stdout, "E-stop trigger operation is in progress, skipping reset."); + } +} + +bool EStopPTH10X::ReadEStopState() +{ + 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_triggered_) { + TriggerEStop(); + } + + return e_stop_triggered_; +} + +void EStopPTH10X::TriggerEStop() +{ + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_); + std::lock_guard lck_g(*motor_controller_write_mtx_); + + try { + motors_controller_->TurnOnSafetyStop(); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Error when trying to set safety stop using CAN command: " + std::string(e.what())); + } + + e_stop_triggered_ = true; +} + +void EStopPTH10X::ResetEStop() +{ + if (e_stop_manipulation_mtx_.try_lock()) { + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); + + if (!ZeroVelocityCheck()) { + throw std::runtime_error( + "Can't reset E-stop - last velocity commands are different than zero. Make sure that your " + "controller sends zero commands before trying to reset E-stop."); + } + + if (!gpio_controller_->IsPinActive(panther_gpiod::GPIOPin::STAGE2_INPUT)) { + throw std::runtime_error("Can't reset E-stop - motors are not powered."); + } + + if (roboteq_error_filter_->IsError()) { + throw std::runtime_error("Can't reset E-stop - motor controller is in an error state."); + } + + roboteq_error_filter_->SetClearErrorsFlag(); + + e_stop_triggered_ = false; + } else { + std::fprintf(stdout, "E-stop trigger operation is in progress, skipping reset."); + } +} + +} // namespace panther_hardware_interfaces