diff --git a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp index c26ceaf..a7fe3f0 100644 --- a/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp +++ b/coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp @@ -17,6 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "std_msgs/msg/string.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/image.hpp" @@ -98,8 +99,24 @@ class InstrumentationLifecycleNode: public rclcpp_lifec private: void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + void handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp::Service::SharedPtr + create_publisher_service_; + rclcpp::Service::SharedPtr + delete_publisher_service_; + + std::unordered_map publishers_; + rclcpp::Node::SharedPtr node_; - image_transport::Publisher pub_; image_transport::Subscriber sub_; std::string topic_; std::string topic_type_; diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp index f9d3cb9..9d3c89b 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp @@ -60,13 +60,19 @@ InstrumentationLifecycleNode::on_configure(const rclcpp_lifecycle::State sub_ = this->create_subscription( 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(*msg)); + for (auto & pub : publishers_) { + if (pub.second->get_subscription_count() > 0 && pub.second->is_activated()) { + pub.second->publish(std::make_unique(*msg)); + } } }); - pub_ = this->create_publisher("/coresense" + topic_, 10); + if (topic_[0] == '/') { + topic_ = topic_.substr(1); + } + + auto pub = this->create_publisher("/coresense/" + topic_, 10); + publishers_.insert({topic_, pub}); create_publisher_service_ = this->create_service( @@ -89,8 +95,9 @@ template typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) { - // TODO: Activar todos los publicadores que haya en publishers_ - pub_->on_activate(); + for (auto & pub : publishers_) { + pub.second->on_activate(); + } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -99,8 +106,9 @@ template typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_deactivate(const rclcpp_lifecycle::State &) { - // TODO: Desactivar todos los publicadores que haya en publishers_ - pub_->on_deactivate(); + for (auto & pub : publishers_) { + pub.second->on_deactivate(); + } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -109,8 +117,10 @@ template typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_cleanup(const rclcpp_lifecycle::State &) { - // TODO: Limpiar todos los publicadores que haya en publishers_ - pub_.reset(); + for (auto & pub : publishers_) { + pub.second.reset(); + } + sub_.reset(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -120,8 +130,10 @@ template typename rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_shutdown(const rclcpp_lifecycle::State &) { - // TODO: Limpiar todos los publicadores que haya en publishers_ - pub_.reset(); + for (auto & pub : publishers_) { + pub.second.reset(); + } + sub_.reset(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -137,12 +149,24 @@ void InstrumentationLifecycleNode::handleCreatePublisherRequest( 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("/coresense" + new_topic, 10); + if (new_topic[0] == '/') { + new_topic = new_topic.substr(1); + } - publishers_.insert({new_topic, new_pub}); + for (auto & pub : publishers_) { + if (pub.first == new_topic) { + response->success = false; + return; + } + } + + auto new_pub = this->create_publisher("/coresense/" + new_topic, 10); + if (this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + new_pub->on_activate(); + } + + publishers_.insert({new_topic, new_pub}); response->success = true; } @@ -156,11 +180,17 @@ void InstrumentationLifecycleNode::handleDeletePublisherRequest( std::string remove_topic = request->topic_name; - // TODO: Mirar si el topic existe - // TODO: Mirar si el topic contiene o no la / + if (remove_topic[0] == '/') { + remove_topic = remove_topic.substr(1); + } - publishers_.erase(remove_topic); + for (auto & pub : publishers_) { + if (pub.first == remove_topic) { + pub.second.reset(); + } + } + publishers_.erase(remove_topic); response->success = true; } diff --git a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp index ded88de..6146b4e 100644 --- a/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp +++ b/coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp @@ -42,8 +42,10 @@ InstrumentationLifecycleNode::~InstrumentationLifecycle void InstrumentationLifecycleNode::imageCallback( const sensor_msgs::msg::Image::ConstSharedPtr & msg) { - if (pub_ && pub_.getNumSubscribers() > 0) { - pub_.publish(msg); + for (auto & pub : publishers_) { + if (pub.second.getNumSubscribers() > 0) { + pub.second.publish(msg); + } } } @@ -69,7 +71,22 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_activate(const rclcpp_lifecycle::State &) { image_transport::ImageTransport it(node_); - pub_ = it.advertise("/coresense" + topic_, 1); + auto pub = it.advertise("/coresense" + topic_, 1); + publishers_.insert({topic_, pub}); + + create_publisher_service_ = + this->create_service( + "/coresense" + topic_ + "/create_publisher", + std::bind( + &InstrumentationLifecycleNode::handleCreatePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + delete_publisher_service_ = + this->create_service( + "/coresense" + topic_ + "/delete_publisher", + std::bind( + &InstrumentationLifecycleNode::handleDeletePublisherRequest, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -78,6 +95,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn InstrumentationLifecycleNode::on_deactivate( const rclcpp_lifecycle::State &) { + publishers_.erase(topic_); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -103,6 +122,49 @@ std::string InstrumentationLifecycleNode::get_topic_typ return topic_type_; } +void InstrumentationLifecycleNode::handleCreatePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string new_topic = request->topic_name; + + if (new_topic[0] == '/') { + new_topic = new_topic.substr(1); + } + + for (auto & pub : publishers_) { + if (pub.first == new_topic) { + response->success = false; + return; + } + } + + image_transport::ImageTransport it(node_); + auto new_pub = it.advertise("/coresense/" + new_topic, 1); + publishers_.insert({new_topic, new_pub}); + response->success = true; +} + +void InstrumentationLifecycleNode::handleDeletePublisherRequest( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string remove_topic = request->topic_name; + + if (remove_topic[0] == '/') { + remove_topic = remove_topic.substr(1); + } + + publishers_.erase(remove_topic); + response->success = true; +} + } // namespace coresense_instrumentation_driver #include "rclcpp_components/register_node_macro.hpp"