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 b87cfc7a1e743..edcabec14930d 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 @@ -109,8 +109,7 @@ class MPTOptimizer const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr); - std::vector optimizeTrajectory( - const PlannerData & planner_data, const std::vector & smoothed_points); + std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); 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 fe7c416799231..9ca2f31b6ec5a 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -37,6 +37,11 @@ class ObstacleAvoidancePlanner : public rclcpp::Node public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // instance. + std::shared_ptr getMPTOptimizer() const { return mpt_optimizer_ptr_; } + + // private: protected: // for the static_centerline_optimizer package // TODO(murooka) move this node to common class DrivingDirectionChecker diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 47f5f59827267..a08c7b3f73973 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -467,8 +467,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; } -std::vector MPTOptimizer::optimizeTrajectory( - const PlannerData & planner_data, const std::vector & smoothed_points) +std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -479,11 +478,11 @@ std::vector MPTOptimizer::optimizeTrajectory( if (prev_optimized_traj_points_ptr_) { return *prev_optimized_traj_points_ptr_; } - return smoothed_points; + return traj_points; }; // 1. calculate reference points - auto ref_points = calcReferencePoints(planner_data, smoothed_points); + auto ref_points = calcReferencePoints(planner_data, traj_points); if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); @@ -817,6 +816,7 @@ void MPTOptimizer::updateBounds( // infeasible to run especially when obstacles are extracted from the drivable area. // In this case, the drivable area's width is forced to be wider. keepMinimumBoundsWidth(ref_points); + // keepMinimumBoundsWidth(ref_points); // extend violated bounds, where the input path is outside the drivable area ref_points = extendViolatedBounds(ref_points); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6d7367af835b6..6a108b94aabd8 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -363,7 +363,7 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area) // with model predictive trajectory - const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points); + const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); time_keeper_ptr_->toc(__func__, " "); return mpt_traj; diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index bbcc8e7c071f6..364e1e3cc43d8 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -36,7 +36,11 @@ class ElasticBandSmoother : public rclcpp::Node public: explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options); -protected: + // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // instance. + std::shared_ptr getElasticBandSmoother() const { return eb_path_smoother_ptr_; } + +private: class DrivingDirectionChecker { public: diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 4731d44ed30bb..a2091dffd061a 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -24,7 +24,6 @@ autoware_package() ament_auto_add_executable(main src/main.cpp src/static_centerline_optimizer_node.cpp - src/successive_trajectory_optimizer_node.cpp src/utils.cpp ) diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index cfee93fbc0bbe..8466b42824d51 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp @@ -52,6 +52,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node // plan path std::vector plan_path(const std::vector & route_lane_ids); + std::vector optimize_trajectory(const Path & raw_path) const; void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp deleted file mode 100644 index 57ebb947b03c2..0000000000000 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// NOTE: This file was copied from a part of implementation in -// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp - -#ifndef STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ - -#include "obstacle_avoidance_planner/node.hpp" -#include "static_centerline_optimizer/type_alias.hpp" - -#include -#include -#include -#include - -namespace static_centerline_optimizer -{ -class SuccessiveTrajectoryOptimizer : public obstacle_avoidance_planner::ObstacleAvoidancePlanner -{ -private: - // subscriber - rclcpp::Subscription::SharedPtr centerline_sub_; - -public: - explicit SuccessiveTrajectoryOptimizer(const rclcpp::NodeOptions & node_options); - - // subscriber callback functions - Trajectory on_centerline(const Path & path); -}; -} // namespace static_centerline_optimizer - -#endif // STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index f727c0e58d327..e915cd0583127 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -17,6 +17,11 @@ + + + diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 89976be5bdd47..51bd9e87d6ba2 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -28,6 +28,7 @@ motion_utils obstacle_avoidance_planner osqp_interface + path_smoother rclcpp rclcpp_components route_handler diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 0aeec7be0b55d..149bcbdd9ead4 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,9 +18,11 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" +#include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" -#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -62,6 +64,21 @@ Path convert_to_path(const PathWithLaneId & path_with_lane_id) return path; } +Trajectory convert_to_trajectory(const Path & path) +{ + Trajectory traj; + for (const auto & point : path.points) { + TrajectoryPoint traj_point; + traj_point.pose = point.pose; + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + + traj.points.push_back(traj_point); + } + return traj; +} + [[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route( const RouteHandler & route_handler, const LaneletRoute & route) { @@ -253,6 +270,7 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ tier4_map_msgs::msg::MapProjectorInfo map_projector_info; map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); if (!map_ptr) { return nullptr; } @@ -388,7 +406,7 @@ std::vector StaticCenterlineOptimizerNode::plan_path( const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, route_lane_ids.front()); - // ego nearest search parameters + // get ego nearest search parameters and resample interval in behavior_path_planner const double ego_nearest_dist_threshold = has_parameter("ego_nearest_dist_threshold") ? get_parameter("ego_nearest_dist_threshold").as_double() @@ -397,31 +415,105 @@ std::vector StaticCenterlineOptimizerNode::plan_path( has_parameter("ego_nearest_yaw_threshold") ? get_parameter("ego_nearest_yaw_threshold").as_double() : declare_parameter("ego_nearest_yaw_threshold"); + const double behavior_path_interval = has_parameter("output_path_interval") + ? get_parameter("output_path_interval").as_double() + : declare_parameter("output_path_interval"); // extract path with lane id from lanelets - const auto raw_path_with_lane_id = utils::get_path_with_lane_id( - *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - + const auto raw_path_with_lane_id = [&]() { + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(get_logger(), "Calculated raw path with lane id and published."); // convert path with lane id to path - const auto raw_path = convert_to_path(raw_path_with_lane_id); + const auto raw_path = [&]() { + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere. + return motion_utils::resamplePath(non_resampled_path, 1.0); + }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(get_logger(), "Converted to path and published."); - // optimize trajectory by the obstacle_avoidance_planner package - SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options()); - const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path); - pub_optimized_centerline_->publish(optimized_traj); - const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj); - - RCLCPP_INFO(get_logger(), "Optimized trajectory and published."); + // smooth trajectory and road collision avoidance + const auto optimized_traj_points = optimize_trajectory(raw_path); + pub_optimized_centerline_->publish( + motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); + RCLCPP_INFO( + get_logger(), "Smoothed trajectory and made it collision free with the road and published."); return optimized_traj_points; } +std::vector StaticCenterlineOptimizerNode::optimize_trajectory( + const Path & raw_path) const +{ + // convert to trajectory points + const auto raw_traj_points = [&]() { + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); + + // create an instance of elastic band and model predictive trajectory. + const auto eb_path_smoother_ptr = + path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + const auto mpt_optimizer_ptr = + obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + + // NOTE: The optimization is executed every valid_optimized_traj_points_num points. + constexpr int valid_optimized_traj_points_num = 10; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + + // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use + // warm start. + constexpr int num_initial_optimization = 2; + + std::vector whole_optimized_traj_points; + for (int virtual_ego_pose_idx = -num_initial_optimization; + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + // calculate virtual ego pose for the optimization + constexpr int virtual_ego_pose_offset_idx = 1; + const auto virtual_ego_pose = + raw_traj_points + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; + + // smooth trajectory by elastic band in the path_smoother package + const auto smoothed_traj_points = + eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, raw_traj_points.front().pose); + + // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // package + const obstacle_avoidance_planner::PlannerData planner_data{ + raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, + smoothed_traj_points.front().pose}; + const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); + + // connect the previously and currently optimized trajectory points + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + break; + } + } + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + return whole_optimized_traj_points; +} + void StaticCenterlineOptimizerNode::on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) { diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp deleted file mode 100644 index a8663abb2eab7..0000000000000 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" - -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include -#include -#include -#include -#include - -namespace static_centerline_optimizer -{ -SuccessiveTrajectoryOptimizer::SuccessiveTrajectoryOptimizer( - const rclcpp::NodeOptions & node_options) -: ObstacleAvoidancePlanner(node_options) -{ - // subscriber - centerline_sub_ = create_subscription( - "debug/raw_centerline", rclcpp::QoS{1}.transient_local(), - std::bind(&SuccessiveTrajectoryOptimizer::on_centerline, this, std::placeholders::_1)); - - // update parameters for replan_checker to execute optimization every cycle - std::vector parameters; - parameters.push_back(rclcpp::Parameter("replan.max_path_shape_around_ego_lat_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_ego_moving_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_goal_moving_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_delta_time_sec", 0.0)); - onParam(parameters); -} - -Trajectory SuccessiveTrajectoryOptimizer::on_centerline(const Path & path) -{ - if (path.points.size() < 2) { - RCLCPP_WARN(get_logger(), "Input path size is less than 2."); - return Trajectory{}; - } - - // parameters for input path sampling - const double resample_interval = 2.0; - const double valid_optimized_path_length = 30.0; - const double path_length = motion_utils::calcArcLength(path.points); - const size_t path_segment_num = static_cast(path_length / valid_optimized_path_length); - - const auto resampled_path = motion_utils::resamplePath(path, resample_interval); // TODO(murooka) - const auto resampled_traj_points = - obstacle_avoidance_planner::trajectory_utils::convertToTrajectoryPoints(resampled_path.points); - - const size_t initial_index = 3; - std::vector whole_optimized_traj_points; - - for (size_t i = 0; i < path_segment_num; ++i) { - // calculate initial pose to start optimization - const auto initial_pose = - resampled_path.points.at(initial_index + valid_optimized_path_length / resample_interval * i) - .pose; - - // create planner data - obstacle_avoidance_planner::PlannerData planner_data; - planner_data.traj_points = resampled_traj_points; - planner_data.left_bound = path.left_bound; - planner_data.right_bound = path.right_bound; - planner_data.ego_pose = initial_pose; - - const auto optimized_traj_points = optimizeTrajectory(planner_data); - - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), whole_optimized_traj_points.begin() + j - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - } - } - - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - // resample - auto output_traj_msg = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(whole_optimized_traj_points), 1.0); - output_traj_msg.header = path.header; - - return output_traj_msg; -} -} // namespace static_centerline_optimizer - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(static_centerline_optimizer::SuccessiveTrajectoryOptimizer) diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 549ddbd7e5245..141743deb007c 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -52,6 +52,14 @@ def generate_test_description(): get_package_share_directory("static_centerline_optimizer"), "config/static_centerline_optimizer.param.yaml", ), + os.path.join( + get_package_share_directory("behavior_path_planner"), + "config/behavior_path_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("path_smoother"), + "config/elastic_band_smoother.param.yaml", + ), os.path.join( get_package_share_directory("obstacle_avoidance_planner"), "config/obstacle_avoidance_planner.param.yaml",