[cooperative perception] Add multiple object tracker node (#2165)

* #2164 Add skeleton for multiple object tracker

The multple object tracker node will ingest detections, execute the
tracking pipeline, then publish the resulting tracks. This commit
covers adds the node skeleton with the implementaiton coming in future
commits.

* #2164 Update function signatures

* #2164 Add functions to create Detections

These functions create CTRV and CTRA detections from ROS 2 messages.

* #2164 Move detection storage into function

The detection storage logic has been moved from the subscription
callback to a separate function because there is some processing that
should happen before the detections are possibly added to the internal
detection lis.

* #2164 Add Track to ROS 2 message function

This function creates ROS2 messages from the tracking pipeline's outputs

* #2164 Add initial multiple object tracker

Add initial multiple object tracker node implementation for performing
the pipeline execution.

* #2164 Add check for existing detection in list

If the detection is already in the list to be processed, the detection's
data is updated instead of being appended to the list.

* #2164 Add dynamic execution frequency

The executing frequency for the tracking pipeline can now be changed
while the node is running.

* #2164 Change track manager type

The multiple object tracker node now uses a simpler track manager that
combines the management policy with the manager itself. This is a change
in the underlying multiple object tracking library. We only needed to
adhere to the new type.

* #2164 Update parameter setting logging

This is really just a cosmetic change.

* #2164 Resolve linting errors

* #2164 Fix bug in track manager instance

The removal threshold was equal to the promotion threshold (and above
the initial occurrance count), so tracks were removed as soon as they
were added to the track manager.

* #2164 Change pipeline logic to always publish

Even if the track list is empty, we should always publish a message to
indicate that the tracker is functioning properly. Additionally, an
empty track list means there are no tracks perceived in the environment.

* #2164 Add clustering to tracking pipeline

We need clustering to choose which detection out of the group will get
turned into a track. Otherwise, we will have duplicate tracks.

* #2164 Add component tests

The component tests use static object scenarios.

* #2164 Fix covariance creation bug

When creating detections from ROS messages, the factory functions did
not properly zero-out the covariance matrices, which resulted in
numerical errors. The covariance matrices should be initialized to
the data from the message, but that is going to be more involved.

* #2164 Add Node documentation

* #2164 Fix typos

* #2164 Add multiple object tracking parameters

The additional parameters allow users to modify the tracker's
promotion and removal thresholds. An example params file has been added
along with a basic launch file.

* #2164 Address peer review about managed pubs/subs

* #2164 Add FetchContent for MOT library

The multiple_object_tracking library was not being installed as a
dependency, which is most likely what is causing the CI build to fail.

* #2164 Update multiple_object_tracking dependency

The carma_cooperative_perception library was previously installing the
library as a source dependency, but now colcon will build it for us.

* #2164 Add MOT library to CARMA checkout script

The multiple_object_tracking library is not available through a
package manager, so it needs to be downloaded from source.

* #2164 Update multiple_object_tracker dependency

The checkout script now downloads from the develop branch. The commented
out FetchContent configurations for carma_cooperative_perception have
been removed.

carma_cooperative_perception/CMakeLists.txt

@@ -63,19 +63,22 @@ ament_auto_add_library(carma_cooperative_perception SHARED
   src/sdsm_to_detection_list_component.cpp
   src/track_list_to_external_object_list_component.cpp
   src/utm_zone.cpp
+  src/multiple_object_tracker_component.cpp
 )
 
 target_link_libraries(carma_cooperative_perception
   ${PROJ4_LIBRARIES}  # Note: Newer versions of PROJ use PROJ::proj
   GSL  # Note: Newer versions of GSL use Microsoft.GSL::GSL
   units::units
+  cooperative_perception_core::cooperative_perception_coreLibrary
 )
 
 rclcpp_components_register_nodes(carma_cooperative_perception
   "carma_cooperative_perception::ExternalObjectListToDetectionListNode"
   "carma_cooperative_perception::ExternalObjectListToSdsmNode"
   "carma_cooperative_perception::SdsmToDetectionListNode"
   "carma_cooperative_perception::TrackListToExternalObjectListComponent"
+  "carma_cooperative_perception::MultipleObjectTrackerComponent"
 )
 
 ament_auto_add_executable(external_object_list_to_detection_list_node
@@ -86,6 +89,10 @@ ament_auto_add_executable(track_list_to_external_object_list_node
   src/track_list_to_external_object_list_node.cpp
 )
 
+ament_auto_add_executable(multiple_object_tracker_node
+  src/multiple_object_tracker_node.cpp
+)
+
 ament_auto_add_executable(external_object_list_to_sdsm_node
   src/external_object_list_to_sdsm_node.cpp
 )
@@ -113,9 +120,17 @@ if(carma_cooperative_perception_BUILD_TESTS)
     carma_cooperative_perception
   )
 
+  add_launch_test(test/track_list_to_external_object_list_launch_test.py)
+  add_launch_test(test/multiple_object_tracker_duplicates_launch_test.py)
+  add_launch_test(test/multiple_object_tracker_static_obstacle_launch_test.py)
+
 endif()
 
-ament_auto_package()
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)
 
 include(GNUInstallDirs)
 

carma_cooperative_perception/config/params.yaml

@@ -0,0 +1,5 @@
+multiple_object_tracker:
+  ros__parameters:
+    execution_frequency_hz: 5.0
+    track_promotion_threshold: 3
+    track_removal_threshold: 0

carma_cooperative_perception/dependencies.cmake

@@ -26,6 +26,8 @@ CPMAddPackage(NAME units
     "BUILD_DOCS FALSE"
 )
 
+find_package(cooperative_perception_core REQUIRED)
+
 # CARMA currently uses PROJ version 6.3.1, which is not designed to be incorporated
 # as a subdirectory into larger projects. If CARMA upgrades to a newer version, we
 # could use the CPMAddPackage(...) command to install PROJ as a source dependency

carma_cooperative_perception/docs/multiple_object_tracker_node.md

@@ -0,0 +1,55 @@
+# Multiple object tracker
+
+This is the main Node in the package; it does the actual object tracking. It wraps the `multiple_object_tracking`
+library in a ROS 2 interface and composes the individual tracking steps into a cohesive pipeline. Check out the
+library's documentation for details on the specific pipeline steps. For our purposes, we need to be concerned only with
+the node's inputs and outputs. It receives incoming `carma_cooperative_perception_interfaces/DetectionList.msg` messages
+from various sources (CARMA Platform and connected remote actors), and sends out
+`carma_cooperative_perception_interfaces/TrackList.msg` messages. This Node fuses the data from the incoming
+`DetectionList.msg` messages to form the outgoing `TrackList.msg` messages' data.
+
+## Node behavior
+
+The `multiple_object_tracker_node` Node executes a multiple object tracking pipeline at a specific frequency, which the
+`execution_frequency_hz` parameter defines. Between executions, the Node listens for incoming `DetectionList.msg`
+messages on the `~/input/detections` topic, queueing received detections for processsing. The tracking pipeline
+executes the following steps on each iteration:
+
+1. temporal alignment
+2. detection-to-track scoring
+3. detection-to-track association
+4. unassociated detection clustering
+5. track maintenance
+6. detection-to-track fusion
+
+The tracker Node outputs a list of confirmed tracks after executing the pipeline.
+
+## Subscriptions
+
+| Topic                | Message Type                                                                           | Description         |
+| -------------------- | -------------------------------------------------------------------------------------- | ------------------- |
+| `~/input/detections` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg_link] | Incoming detections |
+
+[detection_list_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/DetectionList.msg
+
+## Publishers
+
+| Topic             | Message Type                                                                   | Frequency         | Description                                                                                   |
+| ----------------- | ------------------------------------------------------------------------------ | ----------------- | --------------------------------------------------------------------------------------------- |
+| `~/output/tracks` | [`carma_cooperative_perception_interfaces/TrackList.msg`][track_list_msg_link] | Parameter-defined | Tracked objects from the pipeline. **Note:** The track list contains only _confirmed_ tracks. |
+
+[track_list_msg_link]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/TrackList.msg
+
+## Parameters
+
+| Topic                      | Data Type | Default Value | Required | Read Only | Description                                                  |
+| -------------------------- | --------- | ------------- | -------- | --------- | ------------------------------------------------------------ |
+| `~/execution_frequency_hz` | `float`   | `2.0`         | No       | No        | Tracking execution pipeline's execution frequency (in Hertz) |
+
+## Services
+
+This Node does not provide services.
+
+## Actions
+
+This Node does not provide actions.

carma_cooperative_perception/docs/package_design.md

@@ -27,9 +27,11 @@ flow with its neighboring actors (e.g., other connected vehicles or infrastructu
 - External object list to detection list Node:
   [`external_object_list_to_detection_list_node`][external_object_list_to_detection_list_node_docs]
 - Track list to external object list Node: [`track_list_to_external_object_list_node`][track_list_to_external_object_list_node_docs]
+- Multiple object tracker Node: [multiple_object_tracker_node_docs]
 
 [ros2_lifecycle_nodes_link]: https://design.ros2.org/articles/node_lifecycle.html
 [ros2_components_link]: https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Composition.html
 [sdsm_to_detection_list_node_docs]: sdsm_to_detection_list_node.md
 [external_object_list_to_detection_list_node_docs]: external_object_list_to_detection_list_node.md
 [track_list_to_external_object_list_node_docs]: track_list_to_external_object_list_node.md
+[multiple_object_tracker_node_docs]: multiple_object_tracker_node.md

carma_cooperative_perception/include/carma_cooperative_perception/multiple_object_tracker_component.hpp

@@ -0,0 +1,85 @@
+// Copyright 2023 Leidos
+//
+// 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 CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_
+#define CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_
+
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <carma_cooperative_perception_interfaces/msg/detection_list.hpp>
+#include <carma_cooperative_perception_interfaces/msg/track_list.hpp>
+
+#include <cooperative_perception/ctra_model.hpp>
+#include <cooperative_perception/ctrv_model.hpp>
+#include <cooperative_perception/track_management.hpp>
+#include <unordered_map>
+#include <variant>
+#include <vector>
+
+namespace carma_cooperative_perception
+{
+
+using Detection =
+  std::variant<cooperative_perception::CtrvDetection, cooperative_perception::CtraDetection>;
+using Track = std::variant<cooperative_perception::CtrvTrack, cooperative_perception::CtraTrack>;
+
+auto make_detection(const carma_cooperative_perception_interfaces::msg::Detection & msg)
+  -> Detection;
+
+class MultipleObjectTrackerNode : public carma_ros2_utils::CarmaLifecycleNode
+{
+public:
+  explicit MultipleObjectTrackerNode(const rclcpp::NodeOptions & options);
+
+  auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */)
+    -> carma_ros2_utils::CallbackReturn override;
+
+  auto handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */)
+    -> carma_ros2_utils::CallbackReturn override;
+
+  auto handle_on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
+    -> carma_ros2_utils::CallbackReturn override;
+
+  auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */)
+    -> carma_ros2_utils::CallbackReturn override;
+
+  auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */)
+    -> carma_ros2_utils::CallbackReturn override;
+
+  auto store_new_detections(
+    const carma_cooperative_perception_interfaces::msg::DetectionList & msg) noexcept -> void;
+
+  auto execute_pipeline() -> void;
+
+private:
+  rclcpp::Subscription<carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr
+    detection_list_sub_{nullptr};
+
+  rclcpp_lifecycle::LifecyclePublisher<
+    carma_cooperative_perception_interfaces::msg::TrackList>::SharedPtr track_list_pub_{nullptr};
+
+  rclcpp::TimerBase::SharedPtr pipeline_execution_timer_{nullptr};
+
+  std::vector<Detection> detections_;
+  std::unordered_map<cooperative_perception::Uuid, std::size_t> uuid_index_map_;
+  cooperative_perception::FixedThresholdTrackManager<Track> track_manager_{
+    cooperative_perception::PromotionThreshold{3U}, cooperative_perception::RemovalThreshold{0U}};
+  units::time::nanosecond_t execution_period_{1 / units::frequency::hertz_t{2.0}};
+  OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr};
+};
+
+}  // namespace carma_cooperative_perception
+
+#endif  // CARMA_COOPERATIVE_PERCEPTION__MULTIPLE_OBJECT_TRACKER_COMPONENT_HPP_

carma_cooperative_perception/launch/cooperative_perception_launch.py

@@ -0,0 +1,30 @@
+# Copyright 2023 Leidos
+#
+# 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.
+
+from ament_index_python import get_package_share_path
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    package_share_path = get_package_share_path("carma_cooperative_perception")
+
+    multiple_object_tracker_node = Node(
+        package="carma_cooperative_perception",
+        executable="multiple_object_tracker_node",
+        name="multiple_object_tracker",
+        parameters=[package_share_path / "config/params.yaml"],
+    )
+
+    return LaunchDescription([multiple_object_tracker_node])

carma_cooperative_perception/package.xml

@@ -1,5 +1,4 @@
-<?xml version="1.0"?>
-
+<?xml version="1.0" ?>
 <!--
 Copyright 2023 Leidos
 
@@ -15,7 +14,6 @@ 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.
 -->
-
 <package format="3">
   <name>carma_cooperative_perception</name>
   <version>0.1.0</version>
@@ -39,6 +37,7 @@ limitations under the License.
   <depend>lanelet2_core</depend>
   <depend>lanelet2_extension</depend>
   <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>multiple_object_tracking</build_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>