New feature: Create Publishers/Subscribers Service with qos_profile

Signed-off-by: Juancams <[email protected]>
Juancams · Oct 4, 2024 · 799edce · 799edce
1 parent 7f4aa53
commit 799edce
Show file tree

Hide file tree

Showing 4 changed files with 87 additions and 4 deletions.
diff --git a/...umentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp b/...umentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp
@@ -269,7 +269,43 @@ void InstrumentationConsumer<TopicT>::handleCreateSubscriberRequest(
 {
   (void)request_header;
 
+  std::string qos_history = request->qos_history;
+  int qos_queue = request->qos_queue;
+  std::string qos_reliability = request->qos_reliability;
+  std::string qos_durability = request->qos_durability;
   std::string new_topic = request->topic_name;
+  rclcpp::QoS qos_profile = rclcpp::QoS(10);
+
+  if (qos_history == "KEEP_LAST" || qos_history == "") {
+    qos_profile = rclcpp::QoS(rclcpp::KeepLast(qos_queue));
+  } else if (qos_history == "KEEP_ALL") {
+    qos_profile = rclcpp::QoS(rclcpp::KeepAll());
+  } else {
+    response->success = false;
+    response->message = "Invalid queue history. Must be KEEP_LAST or KEEP_ALL";
+    return;
+  }
+
+  if (qos_reliability == "RELIABLE" || qos_reliability == "") {
+    qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+  } else if (qos_reliability == "BEST_EFFORT") {
+    qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
+  } else {
+    response->success = false;
+    response->message = "Invalid reliability. Must be RELIABLE or BEST_EFFORT";
+    return;
+  }
+
+  if (qos_durability == "VOLATILE" || qos_durability == "") {
+    qos_profile.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  } else if (qos_durability == "TRANSIENT_LOCAL") {
+    qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  } else {
+    response->success = false;
+    response->message = "Invalid durability. Must be VOLATILE or TRANSIENT_LOCAL";
+    return;
+  }
+
 
   if (new_topic[0] == '/') {
     new_topic = new_topic.substr(1);
@@ -289,7 +325,7 @@ void InstrumentationConsumer<TopicT>::handleCreateSubscriberRequest(
   }
 
   auto new_sub = this->create_subscription<TopicT>(
-    new_topic, qos_profile_,
+    new_topic, qos_profile,
     [this](const typename TopicT::SharedPtr msg) {
       if (pub_) {
         pub_->publish(std::make_unique<TopicT>(*msg));

diff --git a/...umentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp b/...umentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp
@@ -272,7 +272,42 @@ void InstrumentationProducer<TopicT>::handleCreatePublisherRequest(
 {
   (void)request_header;
 
+  std::string qos_history = request->qos_history;
+  int qos_queue = request->qos_queue;
+  std::string qos_reliability = request->qos_reliability;
+  std::string qos_durability = request->qos_durability;
   std::string new_topic = request->topic_name;
+  rclcpp::QoS qos_profile = rclcpp::QoS(10);
+
+  if (qos_history == "KEEP_LAST" || qos_history == "") {
+    qos_profile = rclcpp::QoS(rclcpp::KeepLast(qos_queue));
+  } else if (qos_history == "KEEP_ALL") {
+    qos_profile = rclcpp::QoS(rclcpp::KeepAll());
+  } else {
+    response->success = false;
+    response->message = "Invalid queue history. Must be KEEP_LAST or KEEP_ALL";
+    return;
+  }
+
+  if (qos_reliability == "RELIABLE" || qos_reliability == "") {
+    qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+  } else if (qos_reliability == "BEST_EFFORT") {
+    qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
+  } else {
+    response->success = false;
+    response->message = "Invalid reliability. Must be RELIABLE or BEST_EFFORT";
+    return;
+  }
+
+  if (qos_durability == "VOLATILE" || qos_durability == "") {
+    qos_profile.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  } else if (qos_durability == "TRANSIENT_LOCAL") {
+    qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  } else {
+    response->success = false;
+    response->message = "Invalid durability. Must be VOLATILE or TRANSIENT_LOCAL";
+    return;
+  }
 
   if (new_topic[0] == '/') {
     new_topic = new_topic.substr(1);
@@ -287,18 +322,20 @@ void InstrumentationProducer<TopicT>::handleCreatePublisherRequest(
   for (auto & pub : publishers_) {
     if (pub.first == new_topic) {
       response->success = false;
+      response->message = "Topic already exists.";
       return;
     }
   }
 
-  auto new_pub = this->create_publisher<TopicT>(new_topic, qos_profile_);
+  auto new_pub = this->create_publisher<TopicT>(new_topic, qos_profile);
 
   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;
+  response->message = "Publisher created.";
 }
 
 template<typename TopicT>

diff --git a/coresense_instrumentation_interfaces/srv/CreatePublisher.srv b/coresense_instrumentation_interfaces/srv/CreatePublisher.srv
@@ -1,4 +1,9 @@
 string topic_name
+string qos_history "KEEP_LAST"
+int32 qos_queue 10
+string qos_reliability "RELIABLE"
+string qos_durability "VOLATILE"
 
 ---
-bool success
+bool success
+string message
diff --git a/coresense_instrumentation_interfaces/srv/CreateSubscriber.srv b/coresense_instrumentation_interfaces/srv/CreateSubscriber.srv
@@ -1,4 +1,9 @@
 string topic_name
+string qos_history "KEEP_LAST"
+int32 qos_queue 10
+string qos_reliability "RELIABLE"
+string qos_durability "VOLATILE"
 
 ---
-bool success
+bool success
+string message