fix parameters issues

usdot-fhwa-stol · Nov 21, 2023 · 6ae49db · 6ae49db
1 parent 88bc4e1
commit 6ae49db
Show file tree

Hide file tree

Showing 8 changed files with 21 additions and 32 deletions.
diff --git 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,8 +91,8 @@ def generate_launch_description():
     carma_wm_ctrl_param_file = os.path.join(
         get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml')
 
-    carma_cooperative_perception_param_file = os.path.join(
-        get_package_share_directory('carma_cooperative_perception'), 'config/params.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 
@@ -416,15 +417,13 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_external_object_list_to_detection_list_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 remappings=[
                     ("input/georeference", "georeference"),
                     ("output/detections", "full_detection_list"),
                     ("input/external_objects", "external_objects"),
                 ],
                 parameters=[ 
-                    carma_cooperative_perception_param_file
                 ]
             ),
             ComposableNode(
@@ -434,7 +433,6 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_external_object_list_to_sdsm_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 remappings=[
                     ("input/georeference", "georeference"),
@@ -443,7 +441,6 @@ def generate_launch_description():
                     ("input/external_objects", "external_objects"),
                 ],
                 parameters=[ 
-                    carma_cooperative_perception_param_file
                 ]
             ),
             ComposableNode(
@@ -453,15 +450,14 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_host_vehicle_filter_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 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=[ 
-                    carma_cooperative_perception_param_file
+                    cp_host_vehicle_filter_node_file
                 ]
             ),
             ComposableNode(
@@ -471,14 +467,12 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_sdsm_to_detection_list_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 remappings=[
                     ("input/sdsm", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_sdsm" ] ),
                     ("output/detections", "full_detection_list"),
                 ],
                 parameters=[ 
-                    carma_cooperative_perception_param_file
                 ]
             ),
             ComposableNode(
@@ -488,14 +482,12 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_track_list_to_external_object_list_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 remappings=[
                     ("input/track_list", "cooperative_perception_track_list"),
                     ("output/external_object_list", "fused_external_objects"),
                 ],
                 parameters=[ 
-                    carma_cooperative_perception_param_file
                 ]
             ),
             ComposableNode(
@@ -505,14 +497,13 @@ def generate_launch_description():
                 extra_arguments=[
                     {'use_intra_process_comms': True}, 
                     {'--log-level' : GetLogLevel('cp_multiple_object_tracker_node', env_log_levels) },
-                    {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper
                 ],
                 remappings=[
                     ("output/track_list", "cooperative_perception_track_list"),
                     ("input/detection_list", "filtered_detection_list"),
                 ],
                 parameters=[ 
-                    carma_cooperative_perception_param_file
+                    cp_multiple_object_tracker_node_file
                 ]
             ),
 

diff --git a/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml b/carma_cooperative_perception/config/cp_host_vehicle_filter_node.yaml
@@ -0,0 +1 @@
+distance_threshold_meters: 2.0
diff --git a/carma_cooperative_perception/config/cp_multiple_object_tracker_node.yaml 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
diff --git a/carma_cooperative_perception/config/params.yaml b/carma_cooperative_perception/config/params.yaml
diff --git 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/...erative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp b/...erative_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
@@ -61,7 +61,10 @@ auto HostVehicleFilterNode::handle_on_configure(
 
   RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");
 
-  declare_parameter("distance_threshold", 0.0);
+  double distance_threshold_meters = 0.0;
+  distance_threshold_meters = declare_parameter("distance_threshold_meters", distance_threshold_meters);
+  get_parameter<double>("distance_threshold_meters", distance_threshold_meters);
+  this->squared_distance_threshold_meters_ = std::pow(distance_threshold_meters, 2);
 
   on_set_parameters_callback_ =
     add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) {
@@ -70,13 +73,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;
           }
@@ -86,11 +89,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;
@@ -167,7 +170,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{

diff --git 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(