diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index 30f06f7b87f81..b2fcf9076a7b3 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -7,12 +7,7 @@ autoware_package() find_package(Eigen3 REQUIRED) ament_auto_add_library(path_smoother SHARED - # node - src/elastic_band_smoother.cpp - # algorithms - src/elastic_band.cpp - # utils - src/utils/trajectory_utils.cpp + DIRECTORY src ) target_include_directories(path_smoother diff --git a/planning/path_smoother/config/elastic_band_smoother.param.yaml b/planning/path_smoother/config/elastic_band_smoother.param.yaml index 730bc3053e7a3..8e77420dd4ae9 100644 --- a/planning/path_smoother/config/elastic_band_smoother.param.yaml +++ b/planning/path_smoother/config/elastic_band_smoother.param.yaml @@ -35,3 +35,13 @@ # nearest search ego_nearest_dist_threshold: 3.0 # [m] ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + # replanning & trimming trajectory param outside algorithm + replan: + enable: true # if true, only perform smoothing when the input changes significantly + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.1 # threshold of path shape change around forward point [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 17c6bc6f162af..94f4d62c4a6fd 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -30,15 +30,15 @@ struct Bounds; struct PlannerData { - // input - Header header; - std::vector traj_points; // converted from the input path - std::vector left_bound; - std::vector right_bound; - - // ego + std::vector traj_points; geometry_msgs::msg::Pose ego_pose; double ego_vel{}; + + PlannerData( + std::vector traj_points_, geometry_msgs::msg::Pose ego_pose_, double ego_vel_) + : traj_points(traj_points_), ego_pose(ego_pose_), ego_vel(ego_vel_) + { + } }; struct TimeKeeper 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 26bc3b71d8245..7941153df3aec 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -18,6 +18,7 @@ #include "motion_utils/motion_utils.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" +#include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -59,6 +60,7 @@ class ElasticBandSmoother : public rclcpp::Node // algorithms std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr replan_checker_ptr_{nullptr}; // parameters CommonParam common_param_{}; diff --git a/planning/path_smoother/include/path_smoother/replan_checker.hpp b/planning/path_smoother/include/path_smoother/replan_checker.hpp new file mode 100644 index 0000000000000..d06cbc093a0c8 --- /dev/null +++ b/planning/path_smoother/include/path_smoother/replan_checker.hpp @@ -0,0 +1,71 @@ +// Copyright 2023 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. + +#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_ + +#include "path_smoother/common_structs.hpp" +#include "path_smoother/type_alias.hpp" + +#include + +#include +#include + +namespace path_smoother +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data) const; + + bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const; + + void updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + bool enable_; + double max_path_shape_around_ego_lat_dist_; + double max_path_shape_forward_lat_dist_; + double max_path_shape_forward_lon_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace path_smoother + +#endif // PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index d6434c3935d1e..75bf1cff20857 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -20,8 +20,6 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 4120b8aba75b8..9f906448551ca 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -87,6 +87,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option eb_path_smoother_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, common_param_, time_keeper_ptr_); + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); // reset planners initializePlanning(); @@ -109,6 +110,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( // parameters for core algorithms eb_path_smoother_ptr_->onParam(parameters); + replan_checker_ptr_->onParam(parameters); // reset planners initializePlanning(); @@ -162,11 +164,29 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); // 1. calculate trajectory with Elastic Band + // 1.a check if replan (= optimization) is required + PlannerData planner_data( + input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + const bool is_replan_required = [&]() { + if (replan_checker_ptr_->isResetRequired(planner_data)) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + return true; + } + // check replan when not resetting previous optimization + return !prev_optimized_traj_points_ptr_ || + replan_checker_ptr_->isReplanRequired(planner_data, now()); + }(); + replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); - auto smoothed_traj_points = - eb_path_smoother_ptr_->smoothTrajectory(input_traj_points, ego_state_ptr_->pose.pose); + auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( + input_traj_points, ego_state_ptr_->pose.pose) + : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); + prev_optimized_traj_points_ptr_ = + std::make_shared>(smoothed_traj_points); + // 2. update velocity applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp new file mode 100644 index 0000000000000..95c4e1eb1c002 --- /dev/null +++ b/planning/path_smoother/src/replan_checker.cpp @@ -0,0 +1,210 @@ +// Copyright 2023 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 "path_smoother/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "path_smoother/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace path_smoother +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + enable_ = node->declare_parameter("replan.enable"); + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_path_shape_forward_lat_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lat_dist"); + max_path_shape_forward_lon_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lon_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "replan.enable", enable_); + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO( + logger_, "Replan with resetting optimization since path shape around ego changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time) const +{ + if (!enable_) return true; + + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) return true; + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) return true; + + // path shape changes + if (isPathForwardChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan since path forward shape changed."); + return true; + } + + return false; +} + +void ReplanChecker::updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time) +{ + const auto & p = planner_data; + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + // update previous information required in this function + if (is_replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate forward point of previous trajectory points + const size_t prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + + // check if distance is larger than the threshold + constexpr double lon_dist_interval = 10.0; + for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; + lon_dist += lon_dist_interval) { + const auto prev_forward_point = + motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + if (!prev_forward_point) { + continue; + } + + // calculate lateral offset of current trajectory points to prev forward point + const auto forward_seg_idx = + motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { + return true; + } + } + + return false; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace path_smoother