Make motion models in motion comp configurable (#2399)

* make motion models in motion comp configurable

* fix build issues and tested

* revert to correct default params

* add white space

motion_computation/config/parameters.yaml

@@ -31,3 +31,10 @@ enable_mobility_path_processing: false
 # Boolean: If true then ExternalObjects generated from sensor data will be processed.
 # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated)
 enable_sensor_processing: false
+
+# Boolean: True if CTRV motion model should be used for the object type. False, if CV should be used:
+enable_ctrv_for_unknown_obj: true
+enable_ctrv_for_motorcycle_obj: true
+enable_ctrv_for_small_vehicle_obj: true
+enable_ctrv_for_large_vehicle_obj: true
+enable_ctrv_for_pedestrian_obj: false

motion_computation/include/motion_computation/motion_computation_config.hpp

@@ -57,6 +57,14 @@ struct Config
  // will occur (objects may be duplicated)
  bool enable_sensor_processing = false;
 
+ // True if CTRV motion model should be used for the object type. 
+ // False, if CV should be used for all following booleans
+ bool enable_ctrv_for_unknown_obj = true;
+ bool enable_ctrv_for_motorcycle_obj = true;
+ bool enable_ctrv_for_small_vehicle_obj = true;
+ bool enable_ctrv_for_large_vehicle_obj = true;
+ bool enable_ctrv_for_pedestrian_obj = false;
+
  // Stream operator for this config
  friend std::ostream & operator<<(std::ostream & output, const Config & c)
  {
@@ -72,6 +80,11 @@ struct Config
  << "enable_psm_processing: " << c.enable_psm_processing << std::endl
  << "enable_mobility_path_processing: " << c.enable_mobility_path_processing << std::endl
  << "enable_sensor_processing: " << c.enable_sensor_processing << std::endl
+ << "enable_ctrv_for_unknown_obj: " << c.enable_ctrv_for_unknown_obj << std::endl
+ << "enable_ctrv_for_motorcycle_obj: " << c.enable_ctrv_for_motorcycle_obj << std::endl
+ << "enable_ctrv_for_small_vehicle_obj: " << c.enable_ctrv_for_small_vehicle_obj << std::endl
+ << "enable_ctrv_for_large_vehicle_obj: " << c.enable_ctrv_for_large_vehicle_obj << std::endl
+ << "enable_ctrv_for_pedestrian_obj: " << c.enable_ctrv_for_pedestrian_obj << std::endl
  << "}" << std::endl;
  return output;
  }

motion_computation/include/motion_computation/motion_computation_worker.hpp

@@ -73,6 +73,9 @@ class MotionComputationWorker
  void setDetectionInputFlags(
  bool enable_sensor_processing, bool enable_bsm_processing, bool enable_psm_processing,
  bool enable_mobility_path_processing);
+ void setDetectionMotionModelFlags(
+ bool enable_ctrv_for_unknown_obj, bool enable_ctrv_for_motorcycle_obj, bool enable_ctrv_for_small_vehicle_obj,
+ bool enable_ctrv_for_large_vehicle_obj, bool enable_ctrv_for_pedestrian_obj);
 
  // callbacks
  void mobilityPathCallback(const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg);
@@ -140,6 +143,13 @@ class MotionComputationWorker
  bool enable_psm_processing_ = false;
  bool enable_mobility_path_processing_ = false;
 
+ // Flags for the different possible motion models for detections
+ bool enable_ctrv_for_unknown_obj_ = true;
+ bool enable_ctrv_for_motorcycle_obj_ = true;
+ bool enable_ctrv_for_small_vehicle_obj_ = true;
+ bool enable_ctrv_for_large_vehicle_obj_ = true;
+ bool enable_ctrv_for_pedestrian_obj_ = false;
+
  // Map frame
  std::string map_frame_id_ = "map";
 

motion_computation/src/motion_computation_node.cpp

@@ -50,6 +50,17 @@ MotionComputationNode::MotionComputationNode(const rclcpp::NodeOptions & options
  "enable_mobility_path_processing", config_.enable_mobility_path_processing);
  config_.enable_sensor_processing =
  declare_parameter<bool>("enable_sensor_processing", config_.enable_sensor_processing);
+ config_.enable_ctrv_for_unknown_obj =
+ declare_parameter<bool>("enable_ctrv_for_unknown_obj", config_.enable_ctrv_for_unknown_obj);
+ config_.enable_ctrv_for_motorcycle_obj =
+ declare_parameter<bool>("enable_ctrv_for_motorcycle_obj", config_.enable_ctrv_for_motorcycle_obj);
+ config_.enable_ctrv_for_small_vehicle_obj =
+ declare_parameter<bool>("enable_ctrv_for_small_vehicle_obj", config_.enable_ctrv_for_small_vehicle_obj);
+ config_.enable_ctrv_for_large_vehicle_obj =
+ declare_parameter<bool>("enable_ctrv_for_large_vehicle_obj", config_.enable_ctrv_for_large_vehicle_obj);
+ config_.enable_ctrv_for_pedestrian_obj =
+ declare_parameter<bool>("enable_ctrv_for_pedestrian_obj", config_.enable_ctrv_for_pedestrian_obj);
+
 }
 
 rcl_interfaces::msg::SetParametersResult MotionComputationNode::parameter_update_callback(
@@ -68,7 +79,12 @@ rcl_interfaces::msg::SetParametersResult MotionComputationNode::parameter_update
  {{"enable_bsm_processing", config_.enable_bsm_processing},
  {"enable_psm_processing", config_.enable_psm_processing},
  {"enable_mobility_path_processing", config_.enable_mobility_path_processing},
- {"enable_sensor_processing", config_.enable_sensor_processing}},
+ {"enable_sensor_processing", config_.enable_sensor_processing},
+ {"enable_ctrv_for_unknown_obj", config_.enable_ctrv_for_unknown_obj},
+ {"enable_ctrv_for_motorcycle_obj", config_.enable_ctrv_for_motorcycle_obj},
+ {"enable_ctrv_for_small_vehicle_obj", config_.enable_ctrv_for_small_vehicle_obj},
+ {"enable_ctrv_for_large_vehicle_obj", config_.enable_ctrv_for_large_vehicle_obj},
+ {"enable_ctrv_for_pedestrian_obj", config_.enable_ctrv_for_pedestrian_obj}},
  parameters);
 
  rcl_interfaces::msg::SetParametersResult result;
@@ -86,6 +102,9 @@ rcl_interfaces::msg::SetParametersResult MotionComputationNode::parameter_update
  motion_worker_.setDetectionInputFlags(
  config_.enable_sensor_processing, config_.enable_bsm_processing,
  config_.enable_psm_processing, config_.enable_mobility_path_processing);
+ motion_worker_.setDetectionMotionModelFlags(
+ config_.enable_ctrv_for_unknown_obj, config_.enable_ctrv_for_motorcycle_obj,
+ config_.enable_ctrv_for_small_vehicle_obj, config_.enable_ctrv_for_large_vehicle_obj, config_.enable_ctrv_for_pedestrian_obj);
  }
 
  return result;

motion_computation/src/motion_computation_worker.cpp

@@ -35,6 +35,11 @@ void MotionComputationWorker::predictionLogic(
  carma_perception_msgs::msg::ExternalObjectList sensor_list;
  sensor_list.header = obj_list->header;
 
+ if (!obj_list || obj_list->objects.empty())
+ {
+ return;
+ }
+
  for (auto obj : obj_list->objects) {
  // Header contains the frame rest of the fields will use
  // obj.header = obj_list.objects[i].header;
@@ -49,18 +54,18 @@ void MotionComputationWorker::predictionLogic(
  bool use_ctrv_model;
 
  if (obj.object_type == obj.UNKNOWN) {
- use_ctrv_model = true;
+ use_ctrv_model = enable_ctrv_for_unknown_obj_;
  } else if (obj.object_type == obj.MOTORCYCLE) {
- use_ctrv_model = true;
+ use_ctrv_model = enable_ctrv_for_motorcycle_obj_;
  } else if (obj.object_type == obj.SMALL_VEHICLE) {
- use_ctrv_model = true;
+ use_ctrv_model = enable_ctrv_for_small_vehicle_obj_;
  } else if (obj.object_type == obj.LARGE_VEHICLE) {
- use_ctrv_model = true;
+ use_ctrv_model = enable_ctrv_for_large_vehicle_obj_;
  } else if (obj.object_type == obj.PEDESTRIAN) {
- use_ctrv_model = false;
+ use_ctrv_model = enable_ctrv_for_pedestrian_obj_;
  } else {
  obj.object_type = obj.UNKNOWN;
- use_ctrv_model = false;
+ use_ctrv_model = enable_ctrv_for_unknown_obj_;
  } // end if-else
 
  if (use_ctrv_model == true) {
@@ -72,6 +77,7 @@ void MotionComputationWorker::predictionLogic(
  obj, prediction_time_step_, prediction_period_, cv_x_accel_noise_, cv_y_accel_noise_,
  prediction_process_noise_max_, prediction_confidence_drop_rate_);
  }
+
  sensor_list.objects.emplace_back(obj);
  } // end for-loop
 
@@ -131,6 +137,7 @@ void MotionComputationWorker::predictionLogic(
  bsm_obj_id_map_.clear();
  psm_list_.objects.clear();
  psm_obj_id_map_.clear();
+
 }
 
 void MotionComputationWorker::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg)
@@ -184,6 +191,17 @@ void MotionComputationWorker::setDetectionInputFlags(
  enable_mobility_path_processing_ = enable_mobility_path_processing;
 }
 
+void MotionComputationWorker::setDetectionMotionModelFlags(
+ bool enable_ctrv_for_unknown_obj, bool enable_ctrv_for_motorcycle_obj, bool enable_ctrv_for_small_vehicle_obj,
+ bool enable_ctrv_for_large_vehicle_obj, bool enable_ctrv_for_pedestrian_obj)
+{
+ enable_ctrv_for_unknown_obj_ = enable_ctrv_for_unknown_obj;
+ enable_ctrv_for_motorcycle_obj_ = enable_ctrv_for_motorcycle_obj;
+ enable_ctrv_for_small_vehicle_obj_ = enable_ctrv_for_small_vehicle_obj;
+ enable_ctrv_for_large_vehicle_obj_ = enable_ctrv_for_large_vehicle_obj;
+ enable_ctrv_for_pedestrian_obj_ = enable_ctrv_for_pedestrian_obj;
+}
+
 void MotionComputationWorker::mobilityPathCallback(
  const carma_v2x_msgs::msg::MobilityPath::UniquePtr msg)
 {