ROS 2 - Update Service Wrapper

panther_hardware_interfaces/include/panther_hardware_interfaces/panther_system_ros_interface.hpp

@@ -61,36 +61,66 @@ struct CANErrors
   bool rear_can_net_err;
 };
 
-template <typename SrvT, typename Func>
-void ProcessServiceCallback(Func function, SrvT response);
-
-class TriggerServiceWrapper
+/**
+ * @brief A wrapper class for ROS services that simplifies the creation and management of a service
+ * server.
+ *
+ * @tparam SrvT The type of the service message, derived from a .srv file by the ROS build system.
+ * @tparam CallbackT The type of the callback function to be invoked on service request. Currently
+ * supported callback function signatures include void() and void(bool).
+ */
+template <typename SrvT, typename CallbackT>
+class ROSServiceWrapper
 {
 public:
-  TriggerServiceWrapper(const std::function<void()> & callback) : callback_(callback){};
+  using SrvSharedPtr = typename rclcpp::Service<SrvT>::SharedPtr;
+  using SrvRequestConstPtr = typename SrvT::Request::ConstSharedPtr;
+  using SrvResponsePtr = typename SrvT::Response::SharedPtr;
 
-  void CallbackWrapper(
-    const TriggerSrv::Request::ConstSharedPtr /* request */,
-    TriggerSrv::Response::SharedPtr response);
+  /**
+   * @brief Constructs a ROSServiceWrapper object with the specified callback function.
+   *
+   * @param callback The callback function to be called whenever the service receives a request.
+   * Currently supported callback function signatures include void() and void(bool).
+   */
+  ROSServiceWrapper(const CallbackT & callback) : callback_(callback) {}
 
-  rclcpp::Service<TriggerSrv>::SharedPtr service;
+  /**
+   * @brief Register and advertises the service with the specified name under the given ROS node.
+   *
+   * @param node The shared pointer to the ROS node under which the service will be advertised.
+   * @param service_name The name of the service. This is the name under which the service will be
+   * advertised over ROS.
+   */
+  void RegisterService(const rclcpp::Node::SharedPtr node, const std::string & service_name);
 
 private:
-  std::function<void()> callback_;
-};
-
-class SetBoolServiceWrapper
-{
-public:
-  SetBoolServiceWrapper(const std::function<void(const bool)> & callback) : callback_(callback){};
-
-  void CallbackWrapper(
-    const SetBoolSrv::Request::ConstSharedPtr request, SetBoolSrv::Response::SharedPtr response);
+  /**
+   * @brief Internal wrapper function for the user-defined callback function.
+   *
+   * @param request Shared pointer to the service request message.
+   * @param response Shared pointer to the service response message.
+   */
+  void CallbackWrapper(SrvRequestConstPtr request, SrvResponsePtr response);
 
-  rclcpp::Service<SetBoolSrv>::SharedPtr service;
+  /**
+   * @brief Specializations of the `ProccessCallback` method for handling specific service request
+   * types.
+   *
+   * These specializations of the `ProccessCallback` method in the `ROSServiceWrapper` template
+   * class are designed to handle service requests for `std_srvs::srv::SetBool` and
+   * `std_srvs::srv::Trigger` services specifically. They extract the necessary information from the
+   * request (if any) and invoke the user-defined callback function accordingly.
+   *
+   * 1. For `std_srvs::srv::SetBool` service requests, it extracts the boolean data from the request
+   * and passes it to the callback function.
+   * 2. For `std_srvs::srv::Trigger` service requests, it simply calls the callback function without
+   * any parameters since `Trigger` requests do not carry additional data.
+   */
+  void ProccessCallback(SrvRequestConstPtr request);
 
-private:
-  std::function<void(const bool)> callback_;
+  SrvSharedPtr service_;
+  CallbackT callback_;
 };
 
 /**
@@ -114,11 +144,27 @@ class PantherSystemRosInterface
   ~PantherSystemRosInterface();
 
   /**
-   * @brief Adds new service server to node
+   * @brief Registers a new service server associated with a specific service type and callback on
+   * the node.
+   *
+   * @tparam SrvT The type of ROS service for which the server is being created. This should
+   * correspond to a predefined service message type.
+   * @tparam CallbackT The type of the callback function that will be executed when the service
+   * receives a request. The function's signature should be compatible with the expected request and
+   * response types for the given service `SrvT`.
+   * @param service_name A string specifying the unique name under which the service will be
+   * advertised on the ROS network.
+   * @param callback The callback function to be executed when the service receives a request. This
+   * function is responsible for processing the incoming request and producing an appropriate
+   * response. Currently supported callback function signatures include void() and void(bool).
    */
-  void AddTriggerService(const std::string service_name, const std::function<void()> & callback);
-  void AddSetBoolService(
-    const std::string service_name, const std::function<void(const bool)> & callback);
+  template <class SrvT, class CallbackT>
+  void AddService(const std::string & service_name, const CallbackT & callback)
+  {
+    auto wrapper = std::make_shared<ROSServiceWrapper<SrvT, CallbackT>>(callback);
+    wrapper->RegisterService(node_, service_name);
+    service_wrappers_storage_.push_back(wrapper);
+  }
 
   /**
    * @brief Updates fault flags, script flags, and runtime errors in the driver state msg
@@ -168,8 +214,7 @@ class PantherSystemRosInterface
   rclcpp::Publisher<BoolMsg>::SharedPtr e_stop_state_publisher_;
   std::unique_ptr<realtime_tools::RealtimePublisher<BoolMsg>> realtime_e_stop_state_publisher_;
 
-  std::vector<std::shared_ptr<TriggerServiceWrapper>> trigger_wrappers_;
-  std::vector<std::shared_ptr<SetBoolServiceWrapper>> set_bool_wrappers_;
+  std::vector<std::shared_ptr<void>> service_wrappers_storage_;
 };
 
 }  // namespace panther_hardware_interfaces

panther_hardware_interfaces/src/panther_system.cpp

@@ -137,26 +137,26 @@ CallbackReturn PantherSystem::on_activate(const rclcpp_lifecycle::State &)
     &PantherSystemRosInterface::PublishIOState, panther_system_ros_interface_,
     std::placeholders::_1));
 
-  panther_system_ros_interface_->AddSetBoolService(
+  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
     "~/motor_power_enable",
     std::bind(&GPIOControllerInterface::MotorPowerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddSetBoolService(
+  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
     "~/fan_enable",
     std::bind(&GPIOControllerInterface::FanEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddSetBoolService(
+  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
     "~/aux_power_enable",
     std::bind(&GPIOControllerInterface::AUXPowerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddSetBoolService(
+  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
     "~/digital_power_enable",
     std::bind(
       &GPIOControllerInterface::DigitalPowerEnable, gpio_controller_, std::placeholders::_1));
-  panther_system_ros_interface_->AddSetBoolService(
+  panther_system_ros_interface_->AddService<SetBoolSrv, std::function<void(bool)>>(
     "~/charger_enable",
     std::bind(&GPIOControllerInterface::ChargerEnable, gpio_controller_, std::placeholders::_1));
 
-  panther_system_ros_interface_->AddTriggerService(
+  panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
     "~/e_stop_trigger", std::bind(&PantherSystem::SetEStop, this));
-  panther_system_ros_interface_->AddTriggerService(
+  panther_system_ros_interface_->AddService<TriggerSrv, std::function<void()>>(
     "~/e_stop_reset", std::bind(&PantherSystem::ResetEStop, this));
 
   const auto io_state = gpio_controller_->QueryControlInterfaceIOStates();

panther_hardware_interfaces/src/panther_system_ros_interface.cpp

@@ -27,11 +27,25 @@
 namespace panther_hardware_interfaces
 {
 
-template <typename SrvT, typename Func>
-void ProcessServiceCallback(Func callback, SrvT response)
+template class ROSServiceWrapper<std_srvs::srv::SetBool, std::function<void(bool)>>;
+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)
+{
+  service_ = node->create_service<SrvT>(
+    service_name, std::bind(
+                    &ROSServiceWrapper<SrvT, CallbackT>::CallbackWrapper, this,
+                    std::placeholders::_1, std::placeholders::_2));
+}
+
+template <typename SrvT, typename CallbackT>
+void ROSServiceWrapper<SrvT, CallbackT>::CallbackWrapper(
+  SrvRequestConstPtr request, SrvResponsePtr response)
 {
   try {
-    callback();
+    ProccessCallback(request);
     response->success = true;
   } catch (const std::exception & err) {
     response->success = false;
@@ -42,16 +56,18 @@ void ProcessServiceCallback(Func callback, SrvT response)
   }
 }
 
-void TriggerServiceWrapper::CallbackWrapper(
-  TriggerSrv::Request::ConstSharedPtr /* request */, TriggerSrv::Response::SharedPtr response)
+template <>
+void ROSServiceWrapper<std_srvs::srv::SetBool, std::function<void(bool)>>::ProccessCallback(
+  SrvRequestConstPtr request)
 {
-  ProcessServiceCallback([this]() { callback_(); }, response);
+  callback_(request->data);
 }
 
-void SetBoolServiceWrapper::CallbackWrapper(
-  SetBoolSrv::Request::ConstSharedPtr request, SetBoolSrv::Response::SharedPtr response)
+template <>
+void ROSServiceWrapper<std_srvs::srv::Trigger, std::function<void()>>::ProccessCallback(
+  SrvRequestConstPtr /* request */)
 {
-  ProcessServiceCallback([this, data = request->data]() { callback_(data); }, response);
+  callback_();
 }
 
 PantherSystemRosInterface::PantherSystemRosInterface(
@@ -97,32 +113,6 @@ PantherSystemRosInterface::~PantherSystemRosInterface()
   node_.reset();
 }
 
-void PantherSystemRosInterface::AddTriggerService(
-  const std::string service_name, const std::function<void()> & callback)
-{
-  auto wrapper = std::make_shared<TriggerServiceWrapper>(callback);
-
-  wrapper->service = node_->create_service<TriggerSrv>(
-    service_name, std::bind(
-                    &TriggerServiceWrapper::CallbackWrapper, wrapper, std::placeholders::_1,
-                    std::placeholders::_2));
-
-  trigger_wrappers_.push_back(wrapper);
-}
-
-void PantherSystemRosInterface::AddSetBoolService(
-  const std::string service_name, const std::function<void(const bool)> & callback)
-{
-  auto wrapper = std::make_shared<SetBoolServiceWrapper>(callback);
-
-  wrapper->service = node_->create_service<SetBoolSrv>(
-    service_name, std::bind(
-                    &SetBoolServiceWrapper::CallbackWrapper, wrapper, std::placeholders::_1,
-                    std::placeholders::_2));
-
-  set_bool_wrappers_.push_back(wrapper);
-}
-
 void PantherSystemRosInterface::UpdateMsgErrorFlags(
   const RoboteqData & front, const RoboteqData & rear)
 {