From 8ccdd28634f699d5551e05b2143fd003f7cb4308 Mon Sep 17 00:00:00 2001 From: Adam Morrissett <22103567+adamlm@users.noreply.github.com> Date: Mon, 4 Dec 2023 13:09:41 -0500 Subject: [PATCH] [cooperative perception] Move motion model mapping default values to `MotionModelMapping` class (#2203) * #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. * #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. * #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. --- .../msg_conversion.hpp | 10 +++--- ...bject_list_to_detection_list_component.cpp | 33 ++++++++----------- .../src/multiple_object_tracker_component.cpp | 2 +- 3 files changed, 19 insertions(+), 26 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..1b98d2fdf7 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_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( 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..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,26 +114,6 @@ 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); - on_set_parameters_callback_ = add_on_set_parameters_callback([this](const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -164,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) + "'");