diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 65b4c9fab3d21..bb64006656216 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,7 +1,7 @@ /**: 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. @@ -9,6 +9,7 @@ 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 index 8753699ab9d4b..ce12cbca8ddb6 100644 --- 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 & 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 index eabbf695496a3..ea2e18b099f1d 100644 --- 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 & 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 index 68e9733bcc12f..cb8e0c05732ef 100644 --- 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 index eb4e028f5245d..daf6beea730d3 100755 --- 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 index ffe6444151cd4..79f3dfd2fe2e4 100644 --- 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 & 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,13 +378,6 @@ 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( @@ -397,25 +385,43 @@ MarkerArray getDebugMarker( 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 index 74711ed886ff0..0a55cc8d91f8e 100644 --- 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 & ref_points void MPTOptimizer::updateExtraPoints(std::vector & 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 & 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 & ref_points, const std::vector & 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 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 index c70b84d8d18d1..b63ce29070583 100644 --- 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 calcSegmentLengthVector(const std::vector & points) +{ + std::vector 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("option.debug.enable_pub_debug_marker"); + enable_pub_extra_debug_marker_ = + declare_parameter("option.debug.enable_pub_extra_debug_marker"); // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); @@ -145,6 +158,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( // parameters for debug marker updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); + updateParam( + parameters, "option.debug.enable_pub_extra_debug_marker", enable_pub_extra_debug_marker_); // parameters for debug info updateParam(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{ - 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{ + 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");