perf(obstacle_avoidance_planner): smaller calculation time (autowaref…

…oundation#4608) * perf(obstacle_avoidance_planner): smaller calculation time Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update script Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Dec 25, 2023 · b20e1c8 · b20e1c8
1 parent ffc356b
commit b20e1c8
Show file tree

Hide file tree

Showing 8 changed files with 118 additions and 55 deletions.
diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
@@ -1,14 +1,15 @@
 /**:
   ros__parameters:
     option:
-      enable_skip_optimization: false                              # skip model predictive trajectory
+      enable_skip_optimization: false                              # skip elastic band and model predictive trajectory
       enable_reset_prev_optimization: false                        # If true, optimization has no fix constraint to the previous result.
       enable_outside_drivable_area_stop: true                      # stop if the ego's trajectory footprint is outside the drivable area
       use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered.
 
       debug:
         # flag to publish
         enable_pub_debug_marker: true           # publish debug marker
+        enable_pub_extra_debug_marker: false    # publish extra debug marker
 
         # flag to show
         enable_debug_info: false
@@ -19,7 +20,7 @@
       output_delta_arc_length: 0.5     #  delta arc length for output trajectory [m]
       output_backward_traj_length: 5.0  # backward length for backward trajectory from base_link [m]
 
-      vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] .
+      vehicle_stop_margin_outside_drivable_area: 0.0 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] .
                                                      # This margin will be realized with delta_arc_length_for_mpt_points m precision.
 
     # replanning & trimming trajectory param outside algorithm
@@ -77,7 +78,7 @@
 
       # avoidance
       avoidance:
-        max_bound_fixing_time: 3.0 # [s]
+        max_bound_fixing_time: 1.0 # [s]
         max_longitudinal_margin_for_bound_violation: 1.0 # [m]
         max_avoidance_cost: 0.5                          # [m]
         avoidance_cost_margin: 0.0                       # [m]

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp
@@ -30,6 +30,6 @@ namespace obstacle_avoidance_planner
 MarkerArray getDebugMarker(
   const DebugData & debug_data,
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & optimized_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info);
+  const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker);
 }  // namespace obstacle_avoidance_planner
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp
@@ -118,6 +118,7 @@ class MPTOptimizer
   void onParam(const std::vector<rclcpp::Parameter> & parameters);
 
   double getTrajectoryLength() const;
+  double getDeltaArcLength() const;
   int getNumberOfPoints() const;
 
 private:

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
@@ -61,6 +61,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
 
   // flags for some functions
   bool enable_pub_debug_marker_;
+  bool enable_pub_extra_debug_marker_;
   bool enable_debug_info_;
   bool enable_outside_drivable_area_stop_;
   bool enable_skip_optimization_;

diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py
@@ -100,7 +100,7 @@ def CallbackCalculationCost(self, msg):
         "-f",
         "--functions",
         type=str,
-        default="onPath, optimizeTrajectory, publishDebugMarkerOfOptimization, solveOsqp",
+        default="onPath, calcReferencePoints, calcOptimizedSteerAngles, publishDebugMarkerOfOptimization",
     )
     parser.add_argument(
         "-d",

diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp
@@ -364,16 +364,11 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray(
 
 MarkerArray getDebugMarker(
   const DebugData & debug_data, const std::vector<TrajectoryPoint> & optimized_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info)
+  const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker)
 {
   MarkerArray marker_array;
 
-  // mpt footprints
-  appendMarkerArray(
-    getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num),
-    &marker_array);
-
-  // bounds lines
+  // bounds line
   appendMarkerArray(
     getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array);
 
@@ -383,39 +378,50 @@ MarkerArray getDebugMarker(
       debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num),
     &marker_array);
 
-  // vehicle circle line
-  appendMarkerArray(
-    getVehicleCircleLinesMarkerArray(
-      debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets,
-      vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"),
-    &marker_array);
-
   // current vehicle circles
   appendMarkerArray(
     getCurrentVehicleCirclesMarkerArray(
       debug_data.ego_pose, debug_data.vehicle_circle_longitudinal_offsets,
       debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3),
     &marker_array);
 
-  // vehicle circles
-  appendMarkerArray(
-    getVehicleCirclesMarkerArray(
-      optimized_points, debug_data.vehicle_circle_longitudinal_offsets,
-      debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles",
-      1.0, 0.3, 0.3),
-    &marker_array);
+  // NOTE: Default debug marker is limited for less calculation time
+  //       Circles visualization is comparatively heavy.
+  if (publish_extra_marker) {
+    // vehicle circles
+    appendMarkerArray(
+      getVehicleCirclesMarkerArray(
+        optimized_points, debug_data.vehicle_circle_longitudinal_offsets,
+        debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num,
+        "vehicle_circles", 1.0, 0.3, 0.3),
+      &marker_array);
 
-  // footprint by drivable area
-  if (debug_data.stop_pose_by_drivable_area) {
+    // mpt footprints
     appendMarkerArray(
-      getFootprintByDrivableAreaMarkerArray(
-        *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0,
-        0.0, 0.0),
+      getFootprintsMarkerArray(
+        optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num),
       &marker_array);
-  }
 
-  // debug text
-  appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array);
+    // vehicle circle line
+    appendMarkerArray(
+      getVehicleCircleLinesMarkerArray(
+        debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets,
+        vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num,
+        "vehicle_circle_lines"),
+      &marker_array);
+
+    // footprint by drivable area
+    if (debug_data.stop_pose_by_drivable_area) {
+      appendMarkerArray(
+        getFootprintByDrivableAreaMarkerArray(
+          *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0,
+          0.0, 0.0),
+        &marker_array);
+    }
+
+    // debug text
+    appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array);
+  }
 
   return marker_array;
 }

diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -687,6 +687,8 @@ void MPTOptimizer::updateDeltaArcLength(std::vector<ReferencePoint> & ref_points
 
 void MPTOptimizer::updateExtraPoints(std::vector<ReferencePoint> & ref_points) const
 {
+  time_keeper_ptr_->tic(__func__);
+
   // alpha
   for (size_t i = 0; i < ref_points.size(); ++i) {
     const auto front_wheel_pos =
@@ -774,6 +776,8 @@ void MPTOptimizer::updateExtraPoints(std::vector<ReferencePoint> & ref_points) c
       }
     }
   }
+
+  time_keeper_ptr_->toc(__func__, "          ");
 }
 
 void MPTOptimizer::updateBounds(
@@ -1273,7 +1277,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix(
   obj_matrix.hessian = H;
   obj_matrix.gradient = g;
 
-  time_keeper_ptr_->toc(__func__, "          ");
+  time_keeper_ptr_->toc(__func__, "        ");
   return obj_matrix;
 }
 
@@ -1325,6 +1329,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix(
     A_rows += N_u;
   }
 
+  // NOTE: The following takes 1 [ms]
   Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v);
   Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF);
   Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF);
@@ -1437,13 +1442,8 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix(
     A_rows_end += N_u;
   }
 
-  ConstraintMatrix constraint_matrix;
-  constraint_matrix.linear = A;
-  constraint_matrix.lower_bound = lb;
-  constraint_matrix.upper_bound = ub;
-
-  time_keeper_ptr_->toc(__func__, "          ");
-  return constraint_matrix;
+  time_keeper_ptr_->toc(__func__, "        ");
+  return ConstraintMatrix{A, lb, ub};
 }
 
 void MPTOptimizer::addSteerWeightR(
@@ -1700,6 +1700,8 @@ void MPTOptimizer::publishDebugTrajectories(
   const std_msgs::msg::Header & header, const std::vector<ReferencePoint> & ref_points,
   const std::vector<TrajectoryPoint> & mpt_traj_points) const
 {
+  time_keeper_ptr_->tic(__func__);
+
   // reference points
   const auto ref_traj = trajectory_utils::createTrajectory(
     header, trajectory_utils::convertToTrajectoryPoints(ref_points));
@@ -1713,6 +1715,8 @@ void MPTOptimizer::publishDebugTrajectories(
   // mpt points
   const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points);
   debug_mpt_traj_pub_->publish(mpt_traj);
+
+  time_keeper_ptr_->toc(__func__, "        ");
 }
 
 std::vector<TrajectoryPoint> MPTOptimizer::extractFixedPoints(
@@ -1738,6 +1742,11 @@ double MPTOptimizer::getTrajectoryLength() const
   return forward_traj_length + backward_traj_length;
 }
 
+double MPTOptimizer::getDeltaArcLength() const
+{
+  return mpt_param_.delta_arc_length;
+}
+
 int MPTOptimizer::getNumberOfPoints() const
 {
   return mpt_param_.num_points;

diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -59,6 +59,17 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point)
   constexpr double zero_vel = 0.0001;
   return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel;
 }
+
+std::vector<double> calcSegmentLengthVector(const std::vector<TrajectoryPoint> & points)
+{
+  std::vector<double> segment_length_vector;
+  for (size_t i = 0; i < points.size() - 1; ++i) {
+    const double segment_length =
+      tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
+    segment_length_vector.push_back(segment_length);
+  }
+  return segment_length_vector;
+}
 }  // namespace
 
 ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options)
@@ -94,6 +105,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
 
     // parameter for debug marker
     enable_pub_debug_marker_ = declare_parameter<bool>("option.debug.enable_pub_debug_marker");
+    enable_pub_extra_debug_marker_ =
+      declare_parameter<bool>("option.debug.enable_pub_extra_debug_marker");
 
     // parameter for debug info
     enable_debug_info_ = declare_parameter<bool>("option.debug.enable_debug_info");
@@ -145,6 +158,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam(
 
   // parameters for debug marker
   updateParam<bool>(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_);
+  updateParam<bool>(
+    parameters, "option.debug.enable_pub_extra_debug_marker", enable_pub_extra_debug_marker_);
 
   // parameters for debug info
   updateParam<bool>(parameters, "option.debug.enable_debug_info", enable_debug_info_);
@@ -363,32 +378,61 @@ void ObstacleAvoidancePlanner::applyInputVelocity(
 
     const size_t ego_seg_idx =
       trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_);
-    return motion_utils::cropForwardPoints(
+    const auto cropped_points = motion_utils::cropForwardPoints(
       input_traj_points, ego_pose.position, ego_seg_idx,
       optimized_traj_length + margin_traj_length);
+
+    if (cropped_points.size() < 2) {
+      return input_traj_points;
+    }
+    return cropped_points;
   }();
 
   // update velocity
-  size_t input_traj_start_idx = 0;
+  const auto segment_length_vec = calcSegmentLengthVector(forward_cropped_input_traj_points);
+  const double mpt_delta_arc_length = mpt_optimizer_ptr_->getDeltaArcLength();
+  size_t input_traj_start_idx = trajectory_utils::findEgoSegmentIndex(
+    forward_cropped_input_traj_points, output_traj_points.front().pose, ego_nearest_param_);
   for (size_t i = 0; i < output_traj_points.size(); i++) {
-    // crop backward for efficient calculation
-    const auto cropped_input_traj_points = std::vector<TrajectoryPoint>{
-      forward_cropped_input_traj_points.begin() + input_traj_start_idx,
-      forward_cropped_input_traj_points.end()};
+    // NOTE: input_traj_start/end_idx is calculated for efficient index calculation
+    const size_t input_traj_end_idx = [&]() {
+      double sum_segment_length = 0.0;
+      for (size_t j = input_traj_start_idx + 1; j < segment_length_vec.size(); ++j) {
+        sum_segment_length += segment_length_vec.at(j);
+        if (mpt_delta_arc_length < sum_segment_length) {
+          return j + 1;
+        }
+      }
+      return forward_cropped_input_traj_points.size() - 1;
+    }();
+
+    const auto nearest_traj_point = [&]() {
+      if (input_traj_start_idx == input_traj_end_idx) {
+        return forward_cropped_input_traj_points.at(input_traj_start_idx);
+      }
 
-    const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex(
-      cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_);
-    input_traj_start_idx = nearest_seg_idx;
+      // crop forward and backward for efficient calculation
+      const auto cropped_input_traj_points = std::vector<TrajectoryPoint>{
+        forward_cropped_input_traj_points.begin() + input_traj_start_idx,
+        forward_cropped_input_traj_points.begin() + input_traj_end_idx + 1};
+      assert(2 <= cropped_input_traj_points.size());
+
+      const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex(
+        cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_);
+      input_traj_start_idx += nearest_seg_idx;
+
+      return cropped_input_traj_points.at(nearest_seg_idx);
+    }();
 
     // calculate velocity with zero order hold
-    const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps;
-    output_traj_points.at(i).longitudinal_velocity_mps = velocity;
+    output_traj_points.at(i).longitudinal_velocity_mps =
+      nearest_traj_point.longitudinal_velocity_mps;
   }
 
   // insert stop point explicitly
   const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points);
   if (stop_idx) {
-    const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose;
+    const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose;
     // NOTE: motion_utils::findNearestSegmentIndex is used instead of
     // trajectory_utils::findEgoSegmentIndex
     //       for the case where input_traj_points is much longer than output_traj_points, and the
@@ -499,7 +543,8 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization(
 
   // debug marker
   time_keeper_ptr_->tic("getDebugMarker");
-  const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_);
+  const auto debug_marker =
+    getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_);
   time_keeper_ptr_->toc("getDebugMarker", "      ");
 
   time_keeper_ptr_->tic("publishDebugMarker");