feat: pre-defined channels, select on launcher

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/multi_object_tracker/config/input_channels.param.yaml

@@ -0,0 +1,38 @@
+/**:
+  ros__parameters:
+    input_channels:
+      detected_objects:
+        topic: "/perception/object_recognition/detection/objects"
+        name: "detected_objects"
+        name_short: "all"
+        expected_frequency: 10.0
+      centerpoint:
+        topic: "/perception/object_recognition/detection/centerpoint/objects"
+        name: "centerpoint"
+        name_short: "Lcp"
+        expected_frequency: 10.0
+      centerpoint_validated:
+        topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
+        name: "centerpoint"
+        name_short: "Lcp"
+        expected_frequency: 10.0
+      camera_lidar_fusion:
+        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
+        name: "camera_lidar_fusion"
+        name_short: "CLf"
+        expected_frequency: 10.0
+      camera_lidar_fusion_filtered:
+        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
+        name: "camera_lidar_fusion"
+        name_short: "CLf"
+        expected_frequency: 10.0
+      detection_by_tracker:
+        topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
+        name: "detection_by_tracker"
+        name_short: "Ld"
+        expected_frequency: 10.0
+      radar_far:
+        topic: "/perception/object_recognition/detection/radar/far_objects"
+        name: "radar_far"
+        name_short: "Rf"
+        expected_frequency: 30.0

perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml

@@ -20,8 +20,3 @@
     publish_debug_markers: false
     diagnostics_warn_delay: 0.5      # [sec]
     diagnostics_error_delay: 1.0     # [sec]
-
-    input_topic_names: ["/perception/object_recognition/detection/objects"]
-    input_names_long: ["detected_objects"]
-    input_names_short: ["all"]
-    input_priority: [0]

perception/multi_object_tracker/launch/multi_object_tracker.launch.xml

@@ -1,14 +1,16 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="input" default="/perception/object_recognition/detection/objects"/>
+  <arg name="selected_input_channels" default="['detected_objects']"/>
   <arg name="output" default="objects"/>
   <arg name="tracker_setting_path" default="$(find-pkg-share multi_object_tracker)/config/multi_object_tracker_node.param.yaml"/>
   <arg name="data_association_matrix_path" default="$(find-pkg-share multi_object_tracker)/config/data_association_matrix.param.yaml"/>
+  <arg name="input_channels_path" default="$(find-pkg-share multi_object_tracker)/config/input_channels.param.yaml"/>
 
   <node pkg="multi_object_tracker" exec="multi_object_tracker" name="multi_object_tracker" output="screen">
-    <remap from="input" to="$(var input)"/>
+    <param name="selected_input_channels" value="$(var selected_input_channels)"/>
     <remap from="output" to="$(var output)"/>
     <param from="$(var tracker_setting_path)"/>
     <param from="$(var data_association_matrix_path)"/>
+    <param from="$(var input_channels_path)"/>
   </node>
 </launch>

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -80,31 +80,38 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
   bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
 
-  declare_parameter("input_topic_names", std::vector<std::string>());
-  declare_parameter("input_names_long", std::vector<std::string>());
-  declare_parameter("input_names_short", std::vector<std::string>());
-  declare_parameter("input_priority", std::vector<int64_t>());
-  std::vector<std::string> input_topic_names = get_parameter("input_topic_names").as_string_array();
-  std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
-  std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
-  std::vector<int64_t> input_priority = get_parameter("input_priority").as_integer_array();
+  declare_parameter("selected_input_channels", std::vector<std::string>());
+  std::vector<std::string> selected_input_channels =
+    get_parameter("selected_input_channels").as_string_array();
 
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
-  // ROS interface - Subscribers
-  if (input_topic_names.empty()) {
+  // ROS interface - Input channels
+  // Get input channels
+  std::vector<std::string> input_topic_names;
+  std::vector<std::string> input_names_long;
+  std::vector<std::string> input_names_short;
+  std::vector<double> input_expected_rates;
+
+  if (selected_input_channels.empty()) {
     RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
     return;
   }
-  if (
-    input_names_long.size() != input_topic_names.size() ||
-    input_names_short.size() != input_topic_names.size() ||
-    input_priority.size() != input_topic_names.size()) {
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "The number of input topic names, long names, and short names must be the same.");
-    return;
+
+  for (const auto & selected_input_channel : selected_input_channels) {
+    const std::string input_topic_name =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".topic");
+    const std::string input_name_long =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name");
+    const std::string input_name_short =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name_short");
+    const double input_expected_frequency =
+      declare_parameter<double>("input_channels." + selected_input_channel + ".expected_frequency");
+    input_topic_names.push_back(input_topic_name);
+    input_names_long.push_back(input_name_long);
+    input_names_short.push_back(input_name_short);
+    input_expected_rates.push_back(input_expected_frequency);
   }
 
   input_channel_size_ = input_topic_names.size();
@@ -114,7 +121,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
-    input_channels_[i].priority = static_cast<int>(input_priority[i]);
+    input_channels_[i].priority = 1;
+    input_channels_[i].expected_rate = input_expected_rates[i];
   }
 
   // Initialize input manager