change constraints evaluation to return vectors

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp

@@ -50,32 +50,31 @@ using SoftConstraintsFunctionVector = std::vector<std::function<double(
 using HardConstraintsFunctionVector = std::vector<std::function<bool(
   sampler_common::Path &, const sampler_common::Constraints &, const MultiPoint2d &)>>;
 
-inline void evaluateSoftConstraints(
+inline std::vector<double> evaluateSoftConstraints(
   sampler_common::Path & path, const sampler_common::Constraints & constraints,
   const SoftConstraintsFunctionVector & soft_constraints_functions,
-  const SoftConstraintsInputs & input_data, std::vector<double> & constraints_results)
+  const SoftConstraintsInputs & input_data)
 {
-  constraints_results.clear();
+  std::vector<double> constraints_results;
   for (const auto & f : soft_constraints_functions) {
     const auto cost = f(path, constraints, input_data);
     constraints_results.push_back(cost);
   }
   if (constraints.soft.weights.size() != constraints_results.size()) {
     path.cost = std::accumulate(constraints_results.begin(), constraints_results.end(), 0.0);
-    return;
+    return constraints_results;
   }
 
   path.cost = std::inner_product(
     constraints_results.begin(), constraints_results.end(), constraints.soft.weights.begin(), 0.0);
-  return;
+  return constraints_results;
 }
 
-inline void evaluateHardConstraints(
+inline std::vector<bool> evaluateHardConstraints(
   sampler_common::Path & path, const sampler_common::Constraints & constraints,
-  const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints,
-  std::vector<bool> & constraints_results)
+  const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints)
 {
-  constraints_results.clear();
+  std::vector<bool> constraints_results;
   bool constraints_passed = true;
   int idx = 0;
   for (const auto & f : hard_constraints) {
@@ -86,7 +85,7 @@ inline void evaluateHardConstraints(
   }
 
   path.constraints_satisfied = constraints_passed;
-  return;
+  return constraints_results;
 }
 
 inline sampler_common::State getInitialState(

planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp

@@ -260,9 +260,8 @@ bool SamplingPlannerModule::isReferencePathSafe() const
         !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
       return path.constraint_results.collision;
     });
-  behavior_path_planner::evaluateHardConstraints(
-    reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path,
-    hard_constraints_results);
+  evaluateHardConstraints(
+    reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path);
   return reference_path.constraints_satisfied;
 }
 
@@ -575,17 +574,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
   std::vector<std::vector<bool>> hard_constraints_results_full;
   std::vector<std::vector<double>> soft_constraints_results_full;
   for (auto & path : frenet_paths) {
-    std::vector<bool> hard_constraints_results;
-    std::vector<double> soft_constraints_results;
     const auto footprint =
       sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints);
-    behavior_path_planner::evaluateHardConstraints(
-      path, internal_params_->constraints, footprint, hard_constraints_, hard_constraints_results);
+    std::vector<bool> hard_constraints_results =
+      evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_);
     path.constraint_results.curvature = true;
     debug_data_.footprints.push_back(footprint);
-    evaluateSoftConstraints(
-      path, internal_params_->constraints, soft_constraints_, soft_constraints_input,
-      soft_constraints_results);
+    std::vector<double> soft_constraints_results = evaluateSoftConstraints(
+      path, internal_params_->constraints, soft_constraints_, soft_constraints_input);
     soft_constraints_results_full.push_back(soft_constraints_results);
   }