Launch cooperative perception nodes (#2195)

* integration_test cp

* fix parameters issues

* revert unnecessary code

carma/launch/environment.launch.py

@@ -23,6 +23,7 @@
 from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace
 from launch.substitutions import LaunchConfiguration
 from launch.actions import DeclareLaunchArgument
+from pathlib import PurePath
 import os
 
 
@@ -90,7 +91,8 @@ def generate_launch_description():
     carma_wm_ctrl_param_file = os.path.join(
         get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml')
 
-
+    cp_multiple_object_tracker_node_file = str(PurePath(get_package_share_directory("carma_cooperative_perception"), "config/cp_multiple_object_tracker_node.yaml"))
+    cp_host_vehicle_filter_node_file = str(PurePath(get_package_share_directory("carma_cooperative_perception"), "config/cp_host_vehicle_filter_node.yaml"))
 
     # lidar_perception_container contains all nodes for lidar based object perception
     # a failure in any one node in the chain would invalidate the rest of it, so they can all be 
@@ -284,7 +286,8 @@ def generate_launch_description():
                     ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ),
                     ("incoming_psm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_psm" ] ),
                     ("incoming_bsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm" ] ),
-                    ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] )
+                    ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ),
+                    ("external_objects", "fused_external_objects")
                 ],
                 parameters=[ 
                     motion_computation_param_file,
@@ -399,6 +402,114 @@ def generate_launch_description():
             )
         ]
     )
+
+    # Cooperative Perception Stack
+    carma_cooperative_perception_container = ComposableNodeContainer(
+        package='carma_ros2_utils', # rclcpp_components
+        name='carma_cooperative_perception_container',
+        executable='carma_component_container_mt',
+        namespace= GetCurrentNamespace(),
+        composable_node_descriptions=[
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::ExternalObjectListToDetectionListNode',
+                name='cp_external_object_list_to_detection_list_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_external_object_list_to_detection_list_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("input/georeference", "georeference"),
+                    ("output/detections", "full_detection_list"),
+                    ("input/external_objects", "external_objects"),
+                ],
+                parameters=[ 
+                ]
+            ),
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::ExternalObjectListToSdsmNode',
+                name='cp_external_object_list_to_sdsm_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_external_object_list_to_sdsm_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("input/georeference", "georeference"),
+                    ("output/sdsms", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_sdsm" ] ),
+                    ("input/pose_stamped", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+                    ("input/external_objects", "external_objects"),
+                ],
+                parameters=[ 
+                ]
+            ),
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::HostVehicleFilterNode',
+                name='cp_host_vehicle_filter_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_host_vehicle_filter_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("input/host_vehicle_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+                    ("input/detection_list", "full_detection_list"),
+                    ("output/detection_list", "filtered_detection_list")
+                ],
+                parameters=[ 
+                    cp_host_vehicle_filter_node_file
+                ]
+            ),
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::SdsmToDetectionListNode',
+                name='cp_sdsm_to_detection_list_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_sdsm_to_detection_list_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("input/sdsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_sdsm" ] ),
+                    ("output/detections", "full_detection_list"),
+                ],
+                parameters=[ 
+                ]
+            ),
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::TrackListToExternalObjectListNode',
+                name='cp_track_list_to_external_object_list_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_track_list_to_external_object_list_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("input/track_list", "cooperative_perception_track_list"),
+                    ("output/external_object_list", "fused_external_objects"),
+                ],
+                parameters=[ 
+                ]
+            ),
+            ComposableNode(
+                package='carma_cooperative_perception',
+                plugin='carma_cooperative_perception::MultipleObjectTrackerNode',
+                name='cp_multiple_object_tracker_node',
+                extra_arguments=[
+                    {'use_intra_process_comms': True}, 
+                    {'--log-level' : GetLogLevel('cp_multiple_object_tracker_node', env_log_levels) },
+                ],
+                remappings=[
+                    ("output/track_list", "cooperative_perception_track_list"),
+                    ("input/detection_list", "filtered_detection_list"),
+                ],
+                parameters=[ 
+                    cp_multiple_object_tracker_node_file
+                ]
+            ),
+
+        ]
+    )
+
     # subsystem_controller which orchestrates the lifecycle of this subsystem's components
     subsystem_controller = Node(
         package='subsystem_controllers',
@@ -418,5 +529,6 @@ def generate_launch_description():
         carma_external_objects_container,
         lanelet2_map_loader_container,
         lanelet2_map_visualization_container,
+        carma_cooperative_perception_container,
         subsystem_controller
     ])

carma_cooperative_perception/CMakeLists.txt

@@ -78,8 +78,9 @@ 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"
+  "carma_cooperative_perception::TrackListToExternalObjectListNode"
+  "carma_cooperative_perception::MultipleObjectTrackerNode"
+  "carma_cooperative_perception::HostVehicleFilterNode"
 )
 
 ament_auto_add_executable(external_object_list_to_detection_list_node

carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml

@@ -0,0 +1 @@
+distance_threshold_meters: 2.0

carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml

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

carma_cooperative_perception/docs/host_vehicle_filter_node.md

@@ -24,7 +24,7 @@ republish the (possibly pruned) list.
 
 | Topic                  | Data Type | Default Value | Required | Read Only | Description                                                                       |
 | ---------------------- | --------- | ------------- | -------- | --------- | --------------------------------------------------------------------------------- |
-| `~/distance_threshold` | `float`   | `0.0`         | No       | No        | Distance below which a detection will be considered to represent the host vehicle |
+| `~/distance_threshold_meters` | `float`   | `0.0`         | No       | No        | Distance below which a detection will be considered to represent the host vehicle |
 
 ## Services
 

carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp

@@ -62,7 +62,7 @@ class HostVehicleFilterNode : public carma_ros2_utils::CarmaLifecycleNode
 
   OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr};
 
-  double squared_distance_threshold_{0.0};
+  double squared_distance_threshold_meters_{0.0};
 };
 
 auto euclidean_distance_squared(

carma_cooperative_perception/src/host_vehicle_filter_component.cpp

@@ -16,6 +16,7 @@
 
 #include <carma_cooperative_perception_interfaces/msg/detection.hpp>
 #include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
 
 #include <vector>
 
@@ -60,7 +61,7 @@ auto HostVehicleFilterNode::handle_on_configure(
 
   RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");
 
-  declare_parameter("distance_threshold", 0.0);
+  declare_parameter("distance_threshold_meters", 0.0);
 
   on_set_parameters_callback_ =
     add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) {
@@ -69,13 +70,13 @@ auto HostVehicleFilterNode::handle_on_configure(
       result.reason = "success";
 
       for (const auto & parameter : parameters) {
-        if (parameter.get_name() == "distance_threshold") {
+        if (parameter.get_name() == "distance_threshold_meters") {
           if (this->get_current_state().label() == "active") {
             result.successful = false;
             result.reason = "parameter is read-only while node is in 'Active' state";
 
             RCLCPP_ERROR(
-              get_logger(), "Cannot change parameter 'distance_threshold': " + result.reason);
+              get_logger(), "Cannot change parameter 'distance_threshold_meters': " + result.reason);
 
             break;
           }
@@ -85,11 +86,11 @@ auto HostVehicleFilterNode::handle_on_configure(
             result.reason = "parameter must be nonnegative";
 
             RCLCPP_ERROR(
-              get_logger(), "Cannot change parameter 'distance_threshold': " + result.reason);
+              get_logger(), "Cannot change parameter 'distance_threshold_meters': " + result.reason);
 
             break;
           } else {
-            this->squared_distance_threshold_ = std::pow(value, 2);
+            this->squared_distance_threshold_meters_ = std::pow(value, 2);
           }
         } else {
           result.successful = false;
@@ -166,7 +167,7 @@ auto HostVehicleFilterNode::attempt_filter_and_republish(
 
   const auto is_within_distance = [this](const auto & detection) {
     return euclidean_distance_squared(host_vehicle_pose_.value().pose, detection.pose.pose) <=
-           this->squared_distance_threshold_;
+           this->squared_distance_threshold_meters_;
   };
 
   const auto new_end{
@@ -188,3 +189,11 @@ auto euclidean_distance_squared(
 }
 
 }  // namespace carma_cooperative_perception
+
+
+// This is not our macro, so we should not worry about linting it.
+// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes
+// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but
+// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0.
+RCLCPP_COMPONENTS_REGISTER_NODE(                               // NOLINT
+  carma_cooperative_perception::HostVehicleFilterNode)  // NOLINT

carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp

@@ -85,3 +85,9 @@ auto TrackListToExternalObjectListNode::publish_as_external_object_list(
 }
 
 }  // namespace carma_cooperative_perception
+
+// This is not our macro, so we should not worry about linting it.
+// clang-tidy added support for ignoring system macros in release 14.0.0 (see the release notes
+// here: https://releases.llvm.org/14.0.0/tools/clang/tools/extra/docs/ReleaseNotes.html), but
+// ament_clang_tidy for ROS 2 Foxy specifically looks for clang-tidy-6.0.
+RCLCPP_COMPONENTS_REGISTER_NODE(carma_cooperative_perception::TrackListToExternalObjectListNode)  // NOLINT

carma_cooperative_perception/test/host_vehicle_filter_launch_test.py

@@ -72,7 +72,7 @@ def generate_test_description():
         package="carma_cooperative_perception",
         executable="host_vehicle_filter_node",
         name="node_under_test",
-        parameters=[{"distance_threshold": 2.0}],
+        parameters=[{"distance_threshold_meters": 2.0}],
     )
 
     launch_description = LaunchDescription(