Merge branch 'develop' into fix/assorted-fix-for-yield

usdot-fhwa-stol · Dec 4, 2023 · a90724d · a90724d
2 parents c512ccb + 8ccdd28
commit a90724d
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 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
@@ -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
@@ -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<rclcpp::Parameter> & 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
@@ -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) + "'");