Merge pull request NVIDIA-ISAAC-ROS#1 from robotperf/main

Updated the ros2_benchmark for PR
mcres · Jun 2, 2023 · 4709046 · 4709046
2 parents 8c10325 + 8c77e2e
commit 4709046
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/ros2_benchmark/include/ros2_benchmark/common.hpp b/ros2_benchmark/include/ros2_benchmark/common.hpp
@@ -27,6 +27,10 @@ namespace ros2_benchmark
 /// ROS 2 intraprocess communications is only compatible with keep_last policy
 const rclcpp::QoS kQoS{{RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1000}};
 
+/// Quality-of-Service policy that 
+const rclcpp::QoS sensorQoS{
+  {RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1000}, rmw_qos_profile_sensor_data};
+
 /// Quality-of-Service policy that will not drop any messages
 const rclcpp::QoS kBufferQoS{
   {RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1000}, rmw_qos_profile_parameters};

diff --git a/ros2_benchmark/include/ros2_benchmark/monitor_node.hpp b/ros2_benchmark/include/ros2_benchmark/monitor_node.hpp
@@ -92,6 +92,9 @@ class MonitorNode : public rclcpp::Node
   /// A service object for StartMonitoring.
   rclcpp::Service<ros2_benchmark_interfaces::srv::StartMonitoring>::SharedPtr
     start_monitoring_service_;
+
+  /// Setting for matching QoS profile between publisher and monitor node
+  std::string qos_type;
 };
 
 }  // namespace ros2_benchmark

diff --git a/ros2_benchmark/src/monitor_node.cpp b/ros2_benchmark/src/monitor_node.cpp
@@ -37,7 +37,8 @@ MonitorNode::MonitorNode(
         std::placeholders::_1,
         std::placeholders::_2),
       rmw_qos_profile_default,
-      service_callback_group_)}
+      service_callback_group_)},
+  qos_type(this->declare_parameter<std::string>("qos_type", "default"))
 {
 }
 
@@ -62,10 +63,15 @@ void MonitorNode::CreateGenericTypeMonitorSubscriber()
     this,
     std::placeholders::_1);
 
+  auto monitor_subs_qos = kQoS;
+  if (qos_type == "sensor"){
+    monitor_subs_qos = sensorQoS;
+  }
+
   monitor_sub_ = this->create_generic_subscription(
     "output",  // topic name
     monitor_data_format_,  // message type in the form of "package/type"
-    kQoS,
+    monitor_subs_qos,
     monitor_subscriber_callback);
 
   RCLCPP_INFO(