templatize node and add merger for tracked objects

Signed-off-by: a-maumau <[email protected]>

Author: a-maumau

perception/autoware_simple_object_merger/CMakeLists.txt

@@ -5,14 +5,25 @@ project(autoware_simple_object_merger)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+include_directories(
+  include
+)
+
 # Targets
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/simple_object_merger_node.cpp
+  src/simple_object_merger_base.cpp
+  src/detected_object_simple_merger/node.cpp
+  src/tracked_object_simple_merger/node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::simple_object_merger::DetectedObjectSimpleMergerNode"
+  EXECUTABLE detected_object_simple_merger_node
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "autoware::simple_object_merger::SimpleObjectMergerNode"
-  EXECUTABLE simple_object_merger_node
+  PLUGIN "autoware::simple_object_merger::TrackedObjectSimpleMergerNode"
+  EXECUTABLE tracked_object_simple_merger_node
 )
 
 # Tests

perception/autoware_simple_object_merger/config/simple_object_merger.param.yaml renamed to perception/autoware_simple_object_merger/config/detected_object_simple_merger.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 20.0
+    new_frame_id: "map"
+    timeout_threshold: 1.0
+    input_topics: [""]
+    uuid_mapping_cleanup_threshold: 30.0 # in sec.

perception/autoware_simple_object_merger/config/tracked_object_simple_merger.param.yaml

@@ -0,0 +1,44 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__SIMPLE_OBJECT_MERGER__DETECTED_OBJECT_SIMPLE_MERGER__NODE_HPP_
+#define AUTOWARE__SIMPLE_OBJECT_MERGER__DETECTED_OBJECT_SIMPLE_MERGER__NODE_HPP_
+
+#include "autoware/simple_object_merger/simple_object_merger_base.hpp"
+#include "autoware_utils/ros/transform_listener.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_perception_msgs/msg/detected_objects.hpp"
+
+namespace autoware::simple_object_merger
+{
+using autoware_perception_msgs::msg::DetectedObjects;
+
+class DetectedObjectSimpleMergerNode
+: public SimpleObjectMergerBase<autoware_perception_msgs::msg::DetectedObjects>
+{
+public:
+  explicit DetectedObjectSimpleMergerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void approximateMerger(
+    const DetectedObjects::ConstSharedPtr & object_msg0,
+    const DetectedObjects::ConstSharedPtr & object_msg1) override;
+
+  void onTimer();
+};
+
+}  // namespace autoware::simple_object_merger
+
+#endif  // AUTOWARE__SIMPLE_OBJECT_MERGER__DETECTED_OBJECT_SIMPLE_MERGER__NODE_HPP_

perception/autoware_simple_object_merger/include/autoware/simple_object_merger/detected_object_simple_merger/node.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2025 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,14 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SIMPLE_OBJECT_MERGER_NODE_HPP_
-#define SIMPLE_OBJECT_MERGER_NODE_HPP_
+#ifndef AUTOWARE__SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_BASE_HPP_
+#define AUTOWARE__SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_BASE_HPP_
 
 #include "autoware_utils/ros/transform_listener.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#include "autoware_perception_msgs/msg/detected_objects.hpp"
-
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
@@ -31,13 +29,13 @@
 
 namespace autoware::simple_object_merger
 {
-using autoware_perception_msgs::msg::DetectedObject;
-using autoware_perception_msgs::msg::DetectedObjects;
 
-class SimpleObjectMergerNode : public rclcpp::Node
+template <class ObjsMsgType>
+class SimpleObjectMergerBase : public rclcpp::Node
 {
 public:
-  explicit SimpleObjectMergerNode(const rclcpp::NodeOptions & node_options);
+  explicit SimpleObjectMergerBase(
+    const std::string & node_name, const rclcpp::NodeOptions & node_options);
 
   struct NodeParam
   {
@@ -49,52 +47,60 @@ class SimpleObjectMergerNode : public rclcpp::Node
 
 private:
   // Subscriber
-  rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
-  std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array{};
-  std::shared_ptr<autoware_utils::TransformListener> transform_listener_;
+  typename rclcpp::Subscription<ObjsMsgType>::SharedPtr sub_objects_{};
+  std::vector<typename rclcpp::Subscription<ObjsMsgType>::SharedPtr> sub_objects_array{};
 
   // Subscriber by message_filter
-  message_filters::Subscriber<DetectedObjects> input0_{};
-  message_filters::Subscriber<DetectedObjects> input1_{};
-  using SyncPolicy =
-    message_filters::sync_policies::ApproximateTime<DetectedObjects, DetectedObjects>;
+  typename message_filters::Subscriber<ObjsMsgType> input0_{};
+  typename message_filters::Subscriber<ObjsMsgType> input1_{};
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<ObjsMsgType, ObjsMsgType>;
   using Sync = message_filters::Synchronizer<SyncPolicy>;
   typename std::shared_ptr<Sync> sync_ptr_;
 
-  // Process callbacks
-  void approximateMerger(
-    const DetectedObjects::ConstSharedPtr & object_msg0,
-    const DetectedObjects::ConstSharedPtr & object_msg1);
-
-  void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number);
-
-  // Data Buffer
-  std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
-  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
-
-  // Publisher
-  rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
-
   // Timer
   rclcpp::TimerBase::SharedPtr timer_{};
-  void onTimer();
-  bool isDataReady();
-  bool shouldLogThrottle(
-    size_t index, const rclcpp::Time & now, std::vector<rclcpp::Time> & last_log_times,
-    double throttle_interval_sec);
 
   // Parameter Server
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  // Process callbacks
+  virtual void approximateMerger(
+    const typename ObjsMsgType::ConstSharedPtr & object_msg0,
+    const typename ObjsMsgType::ConstSharedPtr & object_msg1);
+
+  virtual void onTimer();
+
+  void onData(const typename ObjsMsgType::ConstSharedPtr msg, size_t array_number);
+
   rcl_interfaces::msg::SetParametersResult onSetParam(
     const std::vector<rclcpp::Parameter> & params);
 
+protected:
+  // Publisher
+  typename rclcpp::Publisher<ObjsMsgType>::SharedPtr pub_objects_{};
+
+  std::shared_ptr<autoware_utils::TransformListener> transform_listener_;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
+
+  // Data Buffer
+  std::vector<typename ObjsMsgType::ConstSharedPtr> objects_data_{};
+
+  // Core
+  size_t input_topic_size_;
+
   // Parameter
   NodeParam node_param_{};
 
-  // Core
-  size_t input_topic_size;
+  bool isDataReady();
+  bool shouldLogThrottle(
+    size_t index, const rclcpp::Time & now, std::vector<rclcpp::Time> & last_log_times,
+    double throttle_interval_sec);
+
+  typename ObjsMsgType::SharedPtr getTransformedObjects(
+    typename ObjsMsgType::ConstSharedPtr objects, const std::string & target_frame_id,
+    geometry_msgs::msg::TransformStamped::ConstSharedPtr transform);
 };
 
 }  // namespace autoware::simple_object_merger
 
-#endif  // SIMPLE_OBJECT_MERGER_NODE_HPP_
+#endif  // AUTOWARE__SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_BASE_HPP_

perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp renamed to perception/autoware_simple_object_merger/include/autoware/simple_object_merger/simple_object_merger_base.hpp

@@ -0,0 +1,62 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__SIMPLE_OBJECT_MERGER__TRACKED_OBJECT_SIMPLE_MERGER__NODE_HPP_
+#define AUTOWARE__SIMPLE_OBJECT_MERGER__TRACKED_OBJECT_SIMPLE_MERGER__NODE_HPP_
+
+#include "autoware/simple_object_merger/simple_object_merger_base.hpp"
+#include "autoware_utils/ros/transform_listener.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_perception_msgs/msg/tracked_objects.hpp"
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <string>
+#include <unordered_map>
+
+namespace autoware::simple_object_merger
+{
+using autoware_perception_msgs::msg::TrackedObject;
+using autoware_perception_msgs::msg::TrackedObjects;
+
+struct UUIDMapping
+{
+  int pass_through_node_id = -1;
+  std::unordered_map<int, unique_identifier_msgs::msg::UUID> replace_map;
+  rclcpp::Time last_seen;
+  bool is_mapped_uuid = false;
+};
+
+class TrackedObjectSimpleMergerNode
+: public SimpleObjectMergerBase<autoware_perception_msgs::msg::TrackedObjects>
+{
+public:
+  explicit TrackedObjectSimpleMergerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Duration uuid_mapping_cleanup_threshold_ = rclcpp::Duration::from_seconds(30.0);
+  std::unordered_map<std::string, UUIDMapping> uuid_mapper_;
+
+  void approximateMerger(
+    const TrackedObjects::ConstSharedPtr & object_msg0,
+    const TrackedObjects::ConstSharedPtr & object_msg1) override;
+  void onTimer();
+
+  void mapUUID(TrackedObject & object, const int & node_id);
+  void cleanupUUIDMap();
+};
+
+}  // namespace autoware::simple_object_merger
+
+#endif  // AUTOWARE__SIMPLE_OBJECT_MERGER__TRACKED_OBJECT_SIMPLE_MERGER__NODE_HPP_

perception/autoware_simple_object_merger/include/autoware/simple_object_merger/tracked_object_simple_merger/node.hpp

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="output/objects" default="~/output/objects"/>
+  <arg name="param_path" default="$(find-pkg-share autoware_simple_object_merger)/config/detected_object_simple_merger.param.yaml"/>
+
+  <node pkg="autoware_simple_object_merger" exec="detected_object_simple_merger_node" name="detected_object_simple_merger" output="screen">
+    <remap from="~/output/objects" to="$(var output/objects)"/>
+    <param from="$(var param_path)"/>
+  </node>
+</launch>

perception/autoware_simple_object_merger/launch/detected_object_simple_merger.launch.xml

@@ -1,8 +1,8 @@
 <launch>
   <arg name="output/objects" default="~/output/objects"/>
-  <arg name="param_path" default="$(find-pkg-share autoware_simple_object_merger)/config/simple_object_merger.param.yaml"/>
+  <arg name="param_path" default="$(find-pkg-share autoware_simple_object_merger)/config/tracked_object_simple_merger.param.yaml"/>
 
-  <node pkg="autoware_simple_object_merger" exec="simple_object_merger_node" name="simple_object_merger" output="screen">
+  <node pkg="autoware_simple_object_merger" exec="tracked_object_simple_merger_node" name="tracked_object_simple_merger" output="screen">
     <remap from="~/output/objects" to="$(var output/objects)"/>
     <param from="$(var param_path)"/>
   </node>