Created services

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp

@@ -21,6 +21,8 @@
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "image_transport/image_transport.hpp"
+#include "coresense_instrumentation_interfaces/srv/create_publisher.hpp"
+#include "coresense_instrumentation_interfaces/srv/delete_publisher.hpp"
 
 namespace coresense_instrumentation_driver
 {
@@ -50,6 +52,22 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode
 private:
   typename rclcpp::Subscription<TopicT>::SharedPtr sub_;
   typename rclcpp_lifecycle::LifecyclePublisher<TopicT>::SharedPtr pub_;
+
+  void handleCreatePublisherRequest(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Request> request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Response> response);
+
+  void handleDeletePublisherRequest(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Request> request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response> response);
+
+  rclcpp::Service<coresense_instrumentation_interfaces::srv::CreatePublisher>::SharedPtr create_publisher_service_;
+  rclcpp::Service<coresense_instrumentation_interfaces::srv::DeletePublisher>::SharedPtr delete_publisher_service_;
+
+  std::unordered_map<std::string, typename rclcpp_lifecycle::LifecyclePublisher<TopicT>::SharedPtr> publishers_;
+
   std::string topic_;
   std::string topic_type_;
 };

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp

@@ -61,19 +61,33 @@ InstrumentationLifecycleNode<TopicT>::on_configure(const rclcpp_lifecycle::State
     topic_, 10,
     [this](const typename TopicT::SharedPtr msg) {
       if (pub_) {
+        // TODO: Publicar en todos los publicadores que haya en publishers_
         pub_->publish(std::make_unique<TopicT>(*msg));
       }
     });
 
   pub_ = this->create_publisher<TopicT>("/coresense" + topic_, 10);
 
+  create_publisher_service_ = this->create_service<coresense_instrumentation_interfaces::srv::CreatePublisher>(
+    "/coresense" + topic_ + "/create_publisher",
+    std::bind(
+      &InstrumentationLifecycleNode<TopicT>::handleCreatePublisherRequest, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+  delete_publisher_service_ = this->create_service<coresense_instrumentation_interfaces::srv::DeletePublisher>(
+    "/coresense" + topic_ + "/delete_publisher",
+    std::bind(
+      &InstrumentationLifecycleNode<TopicT>::handleDeletePublisherRequest, this,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
 template<typename TopicT>
 typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 InstrumentationLifecycleNode<TopicT>::on_activate(const rclcpp_lifecycle::State &)
 {
+  // TODO: Activar todos los publicadores que haya en publishers_
   pub_->on_activate();
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -83,6 +97,7 @@ template<typename TopicT>
 typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 InstrumentationLifecycleNode<TopicT>::on_deactivate(const rclcpp_lifecycle::State &)
 {
+  // TODO: Desactivar todos los publicadores que haya en publishers_
   pub_->on_deactivate();
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -92,6 +107,7 @@ template<typename TopicT>
 typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 InstrumentationLifecycleNode<TopicT>::on_cleanup(const rclcpp_lifecycle::State &)
 {
+  // TODO: Limpiar todos los publicadores que haya en publishers_
   pub_.reset();
   sub_.reset();
 
@@ -102,12 +118,50 @@ template<typename TopicT>
 typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 InstrumentationLifecycleNode<TopicT>::on_shutdown(const rclcpp_lifecycle::State &)
 {
+  // TODO: Limpiar todos los publicadores que haya en publishers_
   pub_.reset();
   sub_.reset();
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
+template<typename TopicT>
+void InstrumentationLifecycleNode<TopicT>::handleCreatePublisherRequest(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Request> request,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Response> response)
+{
+  (void)request_header;
+
+  std::string new_topic = request->topic_name;
+
+  // TODO: Mirar si el topic existe
+  // TODO: Mirar si el topic contiene o no la /
+  auto new_pub = this->create_publisher<TopicT>("/coresense" + new_topic, 10);
+
+  publishers_.insert({new_topic, new_pub});
+
+  response->success = true;
+}
+
+template<typename TopicT>
+void InstrumentationLifecycleNode<TopicT>::handleDeletePublisherRequest(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Request> request,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response> response)
+{
+  (void)request_header;
+
+  std::string remove_topic = request->topic_name;
+
+  // TODO: Mirar si el topic existe
+  // TODO: Mirar si el topic contiene o no la /
+
+  publishers_.erase(remove_topic);
+
+  response->success = true;
+}
+
 } // namespace coresense_instrumentation_driver
 
 #include "rclcpp_components/register_node_macro.hpp"