ROS 2 e-stop fixes (#348)

* decrease e-stop reset srv queue size

* add lock for  e-stop read

* fix accidental change

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 <class SrvT, class CallbackT>
   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<ROSServiceWrapper<SrvT, CallbackT>>(callback);
-    wrapper->RegisterService(node_, service_name, callback_group);
+    wrapper->RegisterService(node_, service_name, callback_group, qos_profile);
     service_wrappers_storage_.push_back(wrapper);
   }
 

panther_hardware_interfaces/src/panther_system.cpp

@@ -136,9 +136,12 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &)
   panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
     "~/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<TriggerSrv, std::function<void()>>(
     "~/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);

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<std::mutex> 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<std::mutex> 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_;

panther_hardware_interfaces/src/panther_system_ros_interface.cpp

@@ -33,11 +33,11 @@ template class ROSServiceWrapper<std_srvs::srv::Trigger, std::function<void()>>;
 template <typename SrvT, typename CallbackT>
 void ROSServiceWrapper<SrvT, CallbackT>::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<SrvT>(
     service_name, std::bind(&ROSServiceWrapper<SrvT, CallbackT>::CallbackWrapper, this, _1, _2),
-    rmw_qos_profile_services_default, group);
+    qos_profile, group);
 }
 
 template <typename SrvT, typename CallbackT>
@@ -53,7 +53,7 @@ void ROSServiceWrapper<SrvT, CallbackT>::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());
   }
 }