Skip to content

Commit

Permalink
Services working correctly
Browse files Browse the repository at this point in the history
  • Loading branch information
@Juancams
Juancams committed Dec 11, 2023
1 parent 1161b3b commit 5f910c3
Show file tree
Hide file tree
Showing 3 changed files with 132 additions and 23 deletions.
19 changes: 18 additions & 1 deletion ...entation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -98,8 +99,24 @@ class InstrumentationLifecycleNode<sensor_msgs::msg::Image>: public rclcpp_lifec
private:
void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);

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, image_transport::Publisher> publishers_;

rclcpp::Node::SharedPtr node_;
image_transport::Publisher pub_;
image_transport::Subscriber sub_;
std::string topic_;
std::string topic_type_;
Expand Down
68 changes: 49 additions & 19 deletions ...se_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,13 +60,19 @@ InstrumentationLifecycleNode<TopicT>::on_configure(const rclcpp_lifecycle::State
sub_ = this->create_subscription<TopicT>(
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));
for (auto & pub : publishers_) {
if (pub.second->get_subscription_count() > 0 && pub.second->is_activated()) {
pub.second->publish(std::make_unique<TopicT>(*msg));
}
}
});

pub_ = this->create_publisher<TopicT>("/coresense" + topic_, 10);
if (topic_[0] == '/') {
topic_ = topic_.substr(1);
}

auto pub = this->create_publisher<TopicT>("/coresense/" + topic_, 10);
publishers_.insert({topic_, pub});

create_publisher_service_ =
this->create_service<coresense_instrumentation_interfaces::srv::CreatePublisher>(
Expand All @@ -89,8 +95,9 @@ 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();
for (auto & pub : publishers_) {
pub.second->on_activate();
}

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
Expand All @@ -99,8 +106,9 @@ 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();
for (auto & pub : publishers_) {
pub.second->on_deactivate();
}

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
Expand All @@ -109,8 +117,10 @@ 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();
for (auto & pub : publishers_) {
pub.second.reset();
}

sub_.reset();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
Expand All @@ -120,8 +130,10 @@ 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();
for (auto & pub : publishers_) {
pub.second.reset();
}

sub_.reset();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
Expand All @@ -137,12 +149,24 @@ void InstrumentationLifecycleNode<TopicT>::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<TopicT>("/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<TopicT>("/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;
}

Expand All @@ -156,11 +180,17 @@ void InstrumentationLifecycleNode<TopicT>::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;
}

Expand Down
68 changes: 65 additions & 3 deletions ...ense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,10 @@ InstrumentationLifecycleNode<sensor_msgs::msg::Image>::~InstrumentationLifecycle
void InstrumentationLifecycleNode<sensor_msgs::msg::Image>::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);
}
}
}

Expand All @@ -69,7 +71,22 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
InstrumentationLifecycleNode<sensor_msgs::msg::Image>::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_instrumentation_interfaces::srv::CreatePublisher>(
"/coresense" + topic_ + "/create_publisher",
std::bind(
&InstrumentationLifecycleNode<sensor_msgs::msg::Image>::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<sensor_msgs::msg::Image>::handleDeletePublisherRequest, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
Expand All @@ -78,6 +95,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_deactivate(
const rclcpp_lifecycle::State &)
{
publishers_.erase(topic_);

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand All @@ -103,6 +122,49 @@ std::string InstrumentationLifecycleNode<sensor_msgs::msg::Image>::get_topic_typ
return topic_type_;
}

void InstrumentationLifecycleNode<sensor_msgs::msg::Image>::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;

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<sensor_msgs::msg::Image>::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;

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"
Expand Down

0 comments on commit 5f910c3

Please sign in to comment.