diff --git a/carma/launch/environment.launch.py b/carma/launch/environment.launch.py index 45eed2ac74..e29e839f24 100644 --- a/carma/launch/environment.launch.py +++ b/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 ]) diff --git a/carma_cooperative_perception/CMakeLists.txt b/carma_cooperative_perception/CMakeLists.txt index 229e2cb603..8627b63558 100644 --- a/carma_cooperative_perception/CMakeLists.txt +++ b/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 diff --git a/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml b/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml new file mode 100644 index 0000000000..2c2e5ca84d --- /dev/null +++ b/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml @@ -0,0 +1 @@ +distance_threshold_meters: 2.0 \ No newline at end of file diff --git a/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml b/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml new file mode 100644 index 0000000000..3068edd1ff --- /dev/null +++ b/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 \ No newline at end of file diff --git a/carma_cooperative_perception/config/params.yaml b/carma_cooperative_perception/config/params.yaml deleted file mode 100644 index 973075aafd..0000000000 --- a/carma_cooperative_perception/config/params.yaml +++ /dev/null @@ -1,9 +0,0 @@ -multiple_object_tracker: - ros__parameters: - execution_frequency_hz: 5.0 - track_promotion_threshold: 3 - track_removal_threshold: 0 - -host_vehicle_filter: - ros__parameters: - distance_threshold: 2.0 diff --git a/carma_cooperative_perception/docs/host_vehicle_filter_node.md b/carma_cooperative_perception/docs/host_vehicle_filter_node.md index eaf73752cc..e114a82562 100644 --- a/carma_cooperative_perception/docs/host_vehicle_filter_node.md +++ b/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 diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp index 6f63ef46fb..cc1613845e 100644 --- a/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp +++ b/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( diff --git a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp index c8b8c583d4..562d7937b2 100644 --- a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp +++ b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp @@ -16,6 +16,7 @@ #include #include +#include #include @@ -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 & 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 \ No newline at end of file diff --git a/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp b/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp index fab9770b30..1e7d1f6409 100644 --- a/carma_cooperative_perception/src/track_list_to_external_object_list_component.cpp +++ b/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 \ No newline at end of file diff --git a/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py b/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py index b8e306d434..4dedf8d50c 100644 --- a/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py +++ b/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(