Solved some bugs & lint

Signed-off-by: Juancams <[email protected]>

coresense_instrumentation_driver/config/system.yaml

@@ -6,8 +6,8 @@ nodes:
     qos_profile:
       qos_history: KEEP_LAST
       qos_queue: 100
-      qos_reliability: RELIABLE
-      qos_durability: VOLATILE
+      qos_reliability: BEST_EFFORT
+      qos_durability: TRANSIENT_LOCAL
     topic_name: my_scan
   - name: nav_vel
     topic: /nav_vel

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp

@@ -63,13 +63,17 @@ class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode
 
   void handleCreateSubscriberRequest(
     const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Request> request,
-    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Response> response);
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Request>
+    request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Response>
+    response);
 
   void handleDeleteSubscriberRequest(
     const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request> request,
-    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Response> response);
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request>
+    request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Response>
+    response);
 
   void publish_status();
 

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp

@@ -61,13 +61,17 @@ class InstrumentationProducer : public rclcpp_lifecycle::LifecycleNode
 
   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);
+    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);
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Request>
+    request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response>
+    response);
 
   void publish_status();
 
@@ -113,13 +117,17 @@ class InstrumentationProducer<sensor_msgs::msg::Image>: public rclcpp_lifecycle:
 
   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);
+    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);
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Request>
+    request,
+    const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response>
+    response);
 
   void publish_status();
 

coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py

@@ -13,27 +13,30 @@
 # limitations under the License.
 
 import os
-import yaml
+
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
-from ament_index_python.packages import get_package_share_directory
+import yaml
+
 
 def generate_launch_description():
 
     config_path = os.path.join(
-        get_package_share_directory('coresense_instrumentation_driver'), 
+        get_package_share_directory('coresense_instrumentation_driver'),
         'config',
         'system.yaml'
     )
-    
+
     with open(config_path, 'r') as file:
         config = yaml.safe_load(file)
-    
+
     composable_nodes = []
     ns = ''
 
     for node in config['nodes']:
+
         name = node['name']
         topic = node['topic']
         msg = node['msg']
@@ -46,16 +49,16 @@ def generate_launch_description():
         }
 
         qos_profile = node.get('qos_profile', {})
-        
+
         if 'qos_history' in qos_profile:
             node_params['qos_history'] = qos_profile['qos_history']
-        
+
         if 'qos_queue' in qos_profile:
             node_params['qos_queue'] = qos_profile['qos_queue']
 
         if 'qos_reliability' in qos_profile:
             node_params['qos_reliability'] = qos_profile['qos_reliability']
-        
+
         if 'qos_durability' in qos_profile:
             node_params['qos_durability'] = qos_profile['qos_durability']
 
@@ -68,7 +71,7 @@ def generate_launch_description():
                     + node_type + '<' + msg + '>',
             name=name + '_node',
             namespace=ns,
-            parameters=[node_params], 
+            parameters=[node_params],
         )
 
         composable_nodes.append(composable_node)
@@ -86,4 +89,3 @@ def generate_launch_description():
     ld.add_action(container)
 
     return ld
-

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp

@@ -57,7 +57,7 @@ InstrumentationConsumer<TopicT>::InstrumentationConsumer(
 
   std::string qos_reliability;
   get_parameter("qos_reliability", qos_reliability);
-  
+
   if (qos_reliability == "REALIABLE") {
     qos_profile_.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
   } else if (qos_reliability == "BEST_EFFORT") {
@@ -158,7 +158,7 @@ InstrumentationConsumer<TopicT>::on_configure(const rclcpp_lifecycle::State &)
 
   std::string topic;
 
-  if (topic_name_ != "/") {
+  if (topic_name_ != "") {
     if (topic_name_[0] == '/') {
       topic = topic_name_.substr(1);
     } else {
@@ -264,8 +264,10 @@ InstrumentationConsumer<TopicT>::on_shutdown(const rclcpp_lifecycle::State &)
 template<typename TopicT>
 void InstrumentationConsumer<TopicT>::handleCreateSubscriberRequest(
   const std::shared_ptr<rmw_request_id_t> request_header,
-  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Request> request,
-  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Response> response)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Request>
+  request,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreateSubscriber::Response>
+  response)
 {
   (void)request_header;
 
@@ -306,7 +308,6 @@ void InstrumentationConsumer<TopicT>::handleCreateSubscriberRequest(
     return;
   }
 
-
   if (new_topic[0] == '/') {
     new_topic = new_topic.substr(1);
   }
@@ -339,8 +340,10 @@ void InstrumentationConsumer<TopicT>::handleCreateSubscriberRequest(
 template<typename TopicT>
 void InstrumentationConsumer<TopicT>::handleDeleteSubscriberRequest(
   const std::shared_ptr<rmw_request_id_t> request_header,
-  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request> request,
-  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Response> response)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request>
+  request,
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Response>
+  response)
 {
   (void)request_header;
 

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp

@@ -25,7 +25,7 @@ template class coresense_instrumentation_driver::InstrumentationProducer<geometr
 template<typename TopicT>
 InstrumentationProducer<TopicT>::InstrumentationProducer(
   const rclcpp::NodeOptions & options)
-: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options), qos_profile_(rclcpp::QoS(10)) 
+: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options), qos_profile_(rclcpp::QoS(10))
 {
   declare_parameter("topic", std::string(""));
   declare_parameter("topic_type", std::string(""));
@@ -57,7 +57,7 @@ InstrumentationProducer<TopicT>::InstrumentationProducer(
 
   std::string qos_reliability;
   get_parameter("qos_reliability", qos_reliability);
-  
+
   if (qos_reliability == "RELIABLE") {
     qos_profile_.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
   } else if (qos_reliability == "BEST_EFFORT") {
@@ -166,7 +166,7 @@ InstrumentationProducer<TopicT>::on_configure(const rclcpp_lifecycle::State &)
 
   std::string topic;
 
-  if (topic_name_ != "/") {
+  if (topic_name_ != "") {
     if (topic_name_[0] == '/') {
       topic = topic_name_.substr(1);
     } else {
@@ -268,7 +268,8 @@ template<typename TopicT>
 void InstrumentationProducer<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)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Response>
+  response)
 {
   (void)request_header;
 
@@ -342,7 +343,8 @@ template<typename TopicT>
 void InstrumentationProducer<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)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response>
+  response)
 {
   (void)request_header;
 

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp

@@ -135,7 +135,7 @@ InstrumentationProducer<sensor_msgs::msg::Image>::on_activate(const rclcpp_lifec
   std::string topic;
   std::string create_service, delete_service;
 
-  if (topic_name_ != "/") {
+  if (topic_name_ != "") {
     if (topic_name_[0] == '/') {
       topic = topic_name_.substr(1);
     } else {
@@ -211,7 +211,8 @@ std::string InstrumentationProducer<sensor_msgs::msg::Image>::get_topic_type()
 void InstrumentationProducer<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)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::CreatePublisher::Response>
+  response)
 {
   (void)request_header;
 
@@ -244,7 +245,8 @@ void InstrumentationProducer<sensor_msgs::msg::Image>::handleCreatePublisherRequ
 void InstrumentationProducer<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)
+  const std::shared_ptr<coresense_instrumentation_interfaces::srv::DeletePublisher::Response>
+  response)
 {
   (void)request_header;
 

coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp

@@ -236,7 +236,7 @@ TEST_F(IntegrationTest, CreateStringPublisher)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response->success, true);
+  ASSERT_EQ(response->success, false);
 }
 
 TEST_F(IntegrationTest, DeleteStringPublisher)
@@ -282,7 +282,7 @@ TEST_F(IntegrationTest, DeleteStringPublisher)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response_create->success, true);
+  ASSERT_EQ(response_create->success, false);
 
   auto request_delete =
     std::make_shared<coresense_instrumentation_interfaces::srv::DeletePublisher::Request>();
@@ -309,7 +309,7 @@ TEST_F(IntegrationTest, DeleteStringPublisher)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response_delete->success, true);
+  ASSERT_EQ(response_delete->success, false);
 }
 
 TEST_F(IntegrationTest, CreateTwistSubscription)
@@ -355,7 +355,7 @@ TEST_F(IntegrationTest, CreateTwistSubscription)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response->success, true);
+  ASSERT_EQ(response->success, false);
 }
 
 TEST_F(IntegrationTest, DeleteTwistSubscription)
@@ -401,7 +401,7 @@ TEST_F(IntegrationTest, DeleteTwistSubscription)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response_create->success, true);
+  ASSERT_EQ(response_create->success, false);
 
   auto request_delete =
     std::make_shared<coresense_instrumentation_interfaces::srv::DeleteSubscriber::Request>();
@@ -428,42 +428,7 @@ TEST_F(IntegrationTest, DeleteTwistSubscription)
     executor.spin_once(std::chrono::milliseconds(100));
   }
 
-  ASSERT_EQ(response_delete->success, true);
-}
-
-TEST_F(IntegrationTest, GetStatus)
-{
-  rclcpp::executors::SingleThreadedExecutor executor;
-
-  auto node =
-    std::make_shared<coresense_instrumentation_driver::InstrumentationProducer<std_msgs::msg::String>>(
-    rclcpp::NodeOptions().append_parameter_override(
-      "topic",
-      "/test_topic").append_parameter_override(
-      "topic_type", "std_msgs::msg::String").
-    append_parameter_override("type", "Producer"));
-
-  auto node_sub = rclcpp::Node::make_shared("node_sub");
-
-  executor.add_node(node->get_node_base_interface());
-  executor.add_node(node_sub->get_node_base_interface());
-
-  auto node_info = std::make_shared<coresense_instrumentation_interfaces::msg::NodeInfo>();
-
-  auto sub = node_sub->create_subscription<coresense_instrumentation_interfaces::msg::NodeInfo>(
-    "/status", 10,
-    [&node_info](const coresense_instrumentation_interfaces::msg::NodeInfo::SharedPtr msg) {
-      node_info = std::move(msg);
-    });
-
-  for (int i = 0; i < 30; i++) {
-    executor.spin_once(std::chrono::milliseconds(100));
-  }
-
-  ASSERT_EQ(node_info->node_name, "lifecycle_node");
-  ASSERT_EQ(node_info->type, "Producer");
-  ASSERT_EQ(node_info->state, 1);
-  ASSERT_EQ(node_info->type_msg, "std_msgs::msg::String");
+  ASSERT_EQ(response_delete->success, false);
 }
 
 TEST_F(IntegrationTest, GetTopic)