From 8077443ece8d30b9826e1c66dd0cd350521d8119 Mon Sep 17 00:00:00 2001 From: Adam Morrissett Date: Thu, 30 Nov 2023 17:51:14 -0500 Subject: [PATCH 1/3] #2202 Move default mapping values to class The default motion model mapping values were specified in the ROS parameter declarations, but this meant the MotionModelMapper instance never got them. Its values wouldn't be updated unless a user changed them. Now, the default values are stored in the class instance, and the ROS parameter declarations get their default values from the instance. --- .../msg_conversion.hpp | 10 ++++---- ...bject_list_to_detection_list_component.cpp | 24 ++++--------------- 2 files changed, 10 insertions(+), 24 deletions(-) diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp index 038d8c1240..b4f5523183 100644 --- a/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp +++ b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp @@ -72,11 +72,11 @@ auto to_detection_list_msg(const carma_v2x_msgs::msg::SensorDataSharingMessage & struct MotionModelMapping { - std::uint8_t small_vehicle_model; - std::uint8_t large_vehicle_model; - std::uint8_t motorcycle_model; - std::uint8_t pedestrian_model; - std::uint8_t unknown_model; + std::uint8_t small_vehicle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; + std::uint8_t large_vehicle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; + std::uint8_t motorcycle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA}; + std::uint8_t pedestrian_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; + std::uint8_t unknown_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; }; auto to_detection_msg( diff --git a/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp index eec623d1d5..20bee22713 100644 --- a/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp +++ b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp @@ -114,25 +114,11 @@ auto ExternalObjectListToDetectionListNode::handle_on_configure( } }); - declare_parameter( - "small_vehicle_motion_model", - carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV); - - declare_parameter( - "large_vehicle_motion_model", - carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV); - - declare_parameter( - "motorcycle_motion_model", - carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA); - - declare_parameter( - "pedestrian_motion_model", - carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV); - - declare_parameter( - "unknown_motion_model", - carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV); + declare_parameter("small_vehicle_motion_model", motion_model_mapping_.small_vehicle_model); + declare_parameter("large_vehicle_motion_model", motion_model_mapping_.large_vehicle_model); + declare_parameter("motorcycle_motion_model", motion_model_mapping_.motorcycle_model); + declare_parameter("pedestrian_motion_model", motion_model_mapping_.pedestrian_model); + declare_parameter("unknown_motion_model", motion_model_mapping_.unknown_model); on_set_parameters_callback_ = add_on_set_parameters_callback([this](const std::vector & parameters) { From 87831ce73650ccf4525ef82572549fc13590f843 Mon Sep 17 00:00:00 2001 From: Adam Morrissett Date: Fri, 1 Dec 2023 09:54:38 -0500 Subject: [PATCH 2/3] #2202 Update parameter declaration The parameter declarations for the ExternalObjectListToDetectionListNode got moved to after the on_set_parameter_callback assignment because the callback was previously not being called when the parameters were initially declared. This prevented the motion_model_mapping_ from being updated with the user-overridden mappings. --- ...bject_list_to_detection_list_component.cpp | 19 +++++++++++++------ .../src/multiple_object_tracker_component.cpp | 2 +- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp index 20bee22713..9725f223d7 100644 --- a/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp +++ b/carma_cooperative_perception/src/external_object_list_to_detection_list_component.cpp @@ -114,12 +114,6 @@ auto ExternalObjectListToDetectionListNode::handle_on_configure( } }); - declare_parameter("small_vehicle_motion_model", motion_model_mapping_.small_vehicle_model); - declare_parameter("large_vehicle_motion_model", motion_model_mapping_.large_vehicle_model); - declare_parameter("motorcycle_motion_model", motion_model_mapping_.motorcycle_model); - declare_parameter("pedestrian_motion_model", motion_model_mapping_.pedestrian_model); - declare_parameter("unknown_motion_model", motion_model_mapping_.unknown_model); - on_set_parameters_callback_ = add_on_set_parameters_callback([this](const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -150,6 +144,19 @@ auto ExternalObjectListToDetectionListNode::handle_on_configure( return result; }); + // TODO(Adam Morrissett): Look into how the motion model mapping can be re-architected + // to avoid requiring this sequence. Maybe something like a mapping class that calls the + // declared parameters. Then the to_detected_object() function could be templated on a + // mapping strategy. + + // Declarations must come after on_set_parameters_callback_ assignment because the ROS + // runtime will call the callback if declare_parameter() succeeds. + declare_parameter("small_vehicle_motion_model", motion_model_mapping_.small_vehicle_model); + declare_parameter("large_vehicle_motion_model", motion_model_mapping_.large_vehicle_model); + declare_parameter("motorcycle_motion_model", motion_model_mapping_.motorcycle_model); + declare_parameter("pedestrian_motion_model", motion_model_mapping_.pedestrian_model); + declare_parameter("unknown_motion_model", motion_model_mapping_.unknown_model); + return carma_ros2_utils::CallbackReturn::SUCCESS; } diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index e083ee8f87..5c90eb4251 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -112,7 +112,7 @@ auto make_detection(const carma_cooperative_perception_interfaces::msg::Detectio return make_ctra_detection(msg); case msg.MOTION_MODEL_CV: - break; + throw std::runtime_error("unsupported motion model type '3: constant velocity (CV)'"); } throw std::runtime_error("unkown motion model type '" + std::to_string(msg.motion_model) + "'"); From 989c14d8e906312ceb0601c198906b4fabb8deb7 Mon Sep 17 00:00:00 2001 From: Adam Morrissett Date: Mon, 4 Dec 2023 10:43:09 -0500 Subject: [PATCH 3/3] #2202 Change default motion model mapping The previous motion model mapping used CV model for pedestrians and and unknowns, but the CV model is unsupported. This changes the default to CTRV so we don't default to unsupported motion models. --- .../include/carma_cooperative_perception/msg_conversion.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp index b4f5523183..1b98d2fdf7 100644 --- a/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp +++ b/carma_cooperative_perception/include/carma_cooperative_perception/msg_conversion.hpp @@ -74,9 +74,9 @@ struct MotionModelMapping { std::uint8_t small_vehicle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; std::uint8_t large_vehicle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; - std::uint8_t motorcycle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRA}; - std::uint8_t pedestrian_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; - std::uint8_t unknown_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CV}; + std::uint8_t motorcycle_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; + std::uint8_t pedestrian_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; + std::uint8_t unknown_model{carma_cooperative_perception_interfaces::msg::Detection::MOTION_MODEL_CTRV}; }; auto to_detection_msg(