From e68cedc219b1807db65015f935dafdb401332dfe Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Thu, 20 Jun 2024 10:29:27 +0200 Subject: [PATCH] ROS 2 e-stop fixes (#348) * decrease e-stop reset srv queue size * add lock for e-stop read * fix accidental change --- .../panther_system_ros_interface.hpp | 12 ++++++--- .../src/panther_system.cpp | 5 +++- .../src/panther_system_e_stop.cpp | 26 ++++++++++++------- .../src/panther_system_ros_interface.cpp | 6 ++--- 4 files changed, 32 insertions(+), 17 deletions(-) diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp index cb079e460..5714e175d 100644 --- a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp +++ b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp @@ -97,10 +97,13 @@ class ROSServiceWrapper * advertised over ROS. * @param group The shared pointer to the node's callback group. Defaults to nullptr, which * indicates that the node's default callback group will be used. + * @param qos_profile The QoS settings for the service. Defaults to + * rmw_qos_profile_services_default. */ void RegisterService( const rclcpp::Node::SharedPtr node, const std::string & service_name, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default); private: /** @@ -170,17 +173,20 @@ class PantherSystemRosInterface * enumerated value of `rclcpp::CallbackGroupType`. If a new group must be created, this specifies * whether it should be `MutuallyExclusive` or `Reentrant`. The default value is * `MutuallyExclusive`. + * @param qos_profile The QoS settings for the service. Defaults to + * rmw_qos_profile_services_default. */ template inline void AddService( const std::string & service_name, const CallbackT & callback, const unsigned group_id = 0, - rclcpp::CallbackGroupType callback_group_type = rclcpp::CallbackGroupType::MutuallyExclusive) + rclcpp::CallbackGroupType callback_group_type = rclcpp::CallbackGroupType::MutuallyExclusive, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default) { rclcpp::CallbackGroup::SharedPtr callback_group = GetOrCreateNodeCallbackGroup( group_id, callback_group_type); auto wrapper = std::make_shared>(callback); - wrapper->RegisterService(node_, service_name, callback_group); + wrapper->RegisterService(node_, service_name, callback_group, qos_profile); service_wrappers_storage_.push_back(wrapper); } diff --git a/panther_hardware_interfaces/src/panther_system.cpp b/panther_hardware_interfaces/src/panther_system.cpp index 812f31e55..432019a23 100644 --- a/panther_hardware_interfaces/src/panther_system.cpp +++ b/panther_hardware_interfaces/src/panther_system.cpp @@ -136,9 +136,12 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &) panther_system_ros_interface_->AddService>( "~/e_stop_trigger", std::bind(&EStopInterface::TriggerEStop, e_stop_), 1, rclcpp::CallbackGroupType::MutuallyExclusive); + + auto e_stop_reset_qos = rmw_qos_profile_services_default; + e_stop_reset_qos.depth = 1; panther_system_ros_interface_->AddService>( "~/e_stop_reset", std::bind(&EStopInterface::ResetEStop, e_stop_), 2, - rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroupType::MutuallyExclusive, e_stop_reset_qos); panther_system_ros_interface_->AddDiagnosticTask( std::string("system errors"), this, &PantherSystem::DiagnoseErrors); diff --git a/panther_hardware_interfaces/src/panther_system_e_stop.cpp b/panther_hardware_interfaces/src/panther_system_e_stop.cpp index 4b7de54ef..dc7692f83 100644 --- a/panther_hardware_interfaces/src/panther_system_e_stop.cpp +++ b/panther_hardware_interfaces/src/panther_system_e_stop.cpp @@ -24,13 +24,16 @@ namespace panther_hardware_interfaces bool EStopPTH12X::ReadEStopState() { - e_stop_triggered_ = !gpio_controller_->IsPinActive(GPIOPin::E_STOP_RESET); + if (e_stop_manipulation_mtx_.try_lock()) { + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); + e_stop_triggered_ = !gpio_controller_->IsPinActive(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(); + // 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_; @@ -82,11 +85,14 @@ void EStopPTH12X::ResetEStop() bool EStopPTH10X::ReadEStopState() { - const bool motors_on = gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT); - const bool driver_error = roboteq_error_filter_->IsError(); + if (e_stop_manipulation_mtx_.try_lock()) { + std::lock_guard e_stop_lck(e_stop_manipulation_mtx_, std::adopt_lock); + const bool motors_on = gpio_controller_->IsPinActive(GPIOPin::STAGE2_INPUT); + const bool driver_error = roboteq_error_filter_->IsError(); - if ((driver_error || !motors_on) && !e_stop_triggered_) { - TriggerEStop(); + if ((driver_error || !motors_on) && !e_stop_triggered_) { + TriggerEStop(); + } } return e_stop_triggered_; diff --git a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp index 5db2e6fd1..dfb3e2364 100644 --- a/panther_hardware_interfaces/src/panther_system_ros_interface.cpp +++ b/panther_hardware_interfaces/src/panther_system_ros_interface.cpp @@ -33,11 +33,11 @@ template class ROSServiceWrapper>; template void ROSServiceWrapper::RegisterService( const rclcpp::Node::SharedPtr node, const std::string & service_name, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, const rmw_qos_profile_t & qos_profile) { service_ = node->create_service( service_name, std::bind(&ROSServiceWrapper::CallbackWrapper, this, _1, _2), - rmw_qos_profile_services_default, group); + qos_profile, group); } template @@ -53,7 +53,7 @@ void ROSServiceWrapper::CallbackWrapper( RCLCPP_WARN_STREAM( rclcpp::get_logger("PantherSystem"), - "An exception ocurred while handling the request: " << err.what()); + "An exception occurred while handling the request: " << err.what()); } }