From 8e6c02e0b83d33e80ca1d71cbaf3be4d1c7cc5bd Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 23 Jul 2023 22:31:09 +0900 Subject: [PATCH] feat(path_smoother): make EB's output not std::optional (#4371) * feat(path_smoother): make EB's output not std::optional Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../include/path_smoother/elastic_band.hpp | 14 ++-- .../path_smoother/elastic_band_smoother.hpp | 16 +---- planning/path_smoother/src/elastic_band.cpp | 48 +++++++++----- .../src/elastic_band_smoother.cpp | 65 +++---------------- 4 files changed, 52 insertions(+), 91 deletions(-) diff --git a/planning/path_smoother/include/path_smoother/elastic_band.hpp b/planning/path_smoother/include/path_smoother/elastic_band.hpp index 9b7c4e3deb458..1915757b360a4 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band.hpp @@ -36,8 +36,8 @@ class EBPathSmoother rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const CommonParam & common_param, const std::shared_ptr time_keeper_ptr); - std::optional> getEBTrajectory( - const PlannerData & planner_data); + std::vector smoothTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose); void initialize(const bool enable_debug_info, const CommonParam & common_param); void resetPreviousData(); @@ -103,6 +103,7 @@ class EBPathSmoother EBParam eb_param_; mutable std::shared_ptr time_keeper_ptr_; rclcpp::Logger logger_; + rclcpp::Clock clock_; // publisher rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; @@ -118,13 +119,12 @@ class EBPathSmoother const std::vector & traj_points) const; void updateConstraint( - const std_msgs::msg::Header & header, const std::vector & traj_points, - const bool is_goal_contained, const int pad_start_idx); + const std::vector & traj_points, const bool is_goal_contained, + const int pad_start_idx); - std::optional> optimizeTrajectory(); + std::optional> calcSmoothedTrajectory(); - std::optional> - convertOptimizedPointsToTrajectory( + std::optional> convertOptimizedPointsToTrajectory( const std::vector & optimized_points, const std::vector & traj_points, const int pad_start_idx) const; }; 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 f89d34997a079..26bc3b71d8245 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -96,23 +96,13 @@ class ElasticBandSmoother : public rclcpp::Node // main functions bool isDataReady(const Path & path, rclcpp::Clock clock) const; - PlannerData createPlannerData(const Path & path) const; - std::vector generateOptimizedTrajectory(const PlannerData & planner_data); - std::vector extendTrajectory( - const std::vector & traj_points, - const std::vector & optimized_points) const; - - // functions in generateOptimizedTrajectory - std::vector optimizeTrajectory(const PlannerData & planner_data); - std::vector getPrevOptimizedTrajectory( - const std::vector & traj_points) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const; - void insertZeroVelocityOutsideDrivableArea( - const PlannerData & planner_data, std::vector & traj_points) const; - void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; + std::vector extendTrajectory( + const std::vector & traj_points, + const std::vector & optimized_points) const; }; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index f46c73d8b1e83..d77ad542e2e7e 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -68,6 +68,14 @@ std::vector toStdVector(const Eigen::VectorXd & eigen_vec) { return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; } + +std_msgs::msg::Header createHeader(const rclcpp::Time & now) +{ + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = now; + return header; +} } // namespace namespace path_smoother @@ -153,7 +161,8 @@ EBPathSmoother::EBPathSmoother( ego_nearest_param_(ego_nearest_param), common_param_(common_param), time_keeper_ptr_(time_keeper_ptr), - logger_(node->get_logger().get_child("elastic_band_smoother")) + logger_(node->get_logger().get_child("elastic_band_smoother")), + clock_(*node->get_clock()) { // eb param eb_param_ = EBParam(node); @@ -179,25 +188,30 @@ void EBPathSmoother::resetPreviousData() prev_eb_traj_points_ptr_ = nullptr; } -std::optional> -EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) +std::vector EBPathSmoother::smoothTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose) { time_keeper_ptr_->tic(__func__); - const auto & p = planner_data; + const auto get_prev_eb_traj_points = [&]() { + if (prev_eb_traj_points_ptr_) { + return *prev_eb_traj_points_ptr_; + } + return traj_points; + }; // 1. crop trajectory const double forward_traj_length = eb_param_.num_points * eb_param_.delta_arc_length; const double backward_traj_length = common_param_.output_backward_traj_length; const size_t ego_seg_idx = - trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + trajectory_utils::findEgoSegmentIndex(traj_points, ego_pose, ego_nearest_param_); const auto cropped_traj_points = motion_utils::cropPoints( - p.traj_points, p.ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); + traj_points, ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); // check if goal is contained in cropped_traj_points const bool is_goal_contained = - geometry_utils::isSamePoint(cropped_traj_points.back(), planner_data.traj_points.back()); + geometry_utils::isSamePoint(cropped_traj_points.back(), traj_points.back()); // 2. insert fixed point // NOTE: This should be after cropping trajectory so that fixed point will not be cropped. @@ -221,14 +235,14 @@ EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) const auto [padded_traj_points, pad_start_idx] = getPaddedTrajectoryPoints(resampled_traj_points); // 5. update constraint for elastic band's QP - updateConstraint(p.header, padded_traj_points, is_goal_contained, pad_start_idx); + updateConstraint(padded_traj_points, is_goal_contained, pad_start_idx); // 6. get optimization result - const auto optimized_points = optimizeTrajectory(); + const auto optimized_points = calcSmoothedTrajectory(); if (!optimized_points) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since smoothing failed"); - return std::nullopt; + return get_prev_eb_traj_points(); } // 7. convert optimization result to trajectory @@ -236,13 +250,14 @@ EBPathSmoother::getEBTrajectory(const PlannerData & planner_data) convertOptimizedPointsToTrajectory(*optimized_points, padded_traj_points, pad_start_idx); if (!eb_traj_points) { RCLCPP_WARN(logger_, "return std::nullopt since x or y error is too large"); - return std::nullopt; + return get_prev_eb_traj_points(); } prev_eb_traj_points_ptr_ = std::make_shared>(*eb_traj_points); // 8. publish eb trajectory - const auto eb_traj = trajectory_utils::createTrajectory(p.header, *eb_traj_points); + const auto eb_traj = + trajectory_utils::createTrajectory(createHeader(clock_.now()), *eb_traj_points); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -287,8 +302,8 @@ std::tuple, size_t> EBPathSmoother::getPaddedTrajec } void EBPathSmoother::updateConstraint( - const std_msgs::msg::Header & header, const std::vector & traj_points, - const bool is_goal_contained, const int pad_start_idx) + const std::vector & traj_points, const bool is_goal_contained, + const int pad_start_idx) { time_keeper_ptr_->tic(__func__); @@ -365,13 +380,14 @@ void EBPathSmoother::updateConstraint( } // publish fixed trajectory - const auto eb_fixed_traj = trajectory_utils::createTrajectory(header, debug_fixed_traj_points); + const auto eb_fixed_traj = + trajectory_utils::createTrajectory(createHeader(clock_.now()), debug_fixed_traj_points); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); } -std::optional> EBPathSmoother::optimizeTrajectory() +std::optional> EBPathSmoother::calcSmoothedTrajectory() { time_keeper_ptr_->tic(__func__); diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6546995f1013a..4120b8aba75b8 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -159,14 +159,19 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) return; } - // 1. create planner data - const auto planner_data = createPlannerData(*path_ptr); + const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - // 2. generate optimized trajectory - const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); + // 1. calculate trajectory with Elastic Band + time_keeper_ptr_->tic(__func__); + auto smoothed_traj_points = + eb_path_smoother_ptr_->smoothTrajectory(input_traj_points, ego_state_ptr_->pose.pose); + time_keeper_ptr_->toc(__func__, " "); + + // 2. update velocity + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly - auto full_traj_points = extendTrajectory(planner_data.traj_points, optimized_traj_points); + auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); // 4. set zero velocity after stop point setZeroVelocityAfterStopPoint(full_traj_points); @@ -208,56 +213,6 @@ bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) co return true; } -PlannerData ElasticBandSmoother::createPlannerData(const Path & path) const -{ - // create planner data - PlannerData planner_data; - planner_data.header = path.header; - planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); - planner_data.left_bound = path.left_bound; - planner_data.right_bound = path.right_bound; - planner_data.ego_pose = ego_state_ptr_->pose.pose; - planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; - return planner_data; -} - -std::vector ElasticBandSmoother::generateOptimizedTrajectory( - const PlannerData & planner_data) -{ - time_keeper_ptr_->tic(__func__); - - const auto & input_traj_points = planner_data.traj_points; - - // 1. calculate trajectory with Elastic Band - auto optimized_traj_points = optimizeTrajectory(planner_data); - - // 2. update velocity - applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); - - time_keeper_ptr_->toc(__func__, " "); - return optimized_traj_points; -} - -std::vector ElasticBandSmoother::optimizeTrajectory( - const PlannerData & planner_data) -{ - time_keeper_ptr_->tic(__func__); - const auto & p = planner_data; - - const auto eb_traj = eb_path_smoother_ptr_->getEBTrajectory(planner_data); - if (!eb_traj) return getPrevOptimizedTrajectory(p.traj_points); - - time_keeper_ptr_->toc(__func__, " "); - return *eb_traj; -} - -std::vector ElasticBandSmoother::getPrevOptimizedTrajectory( - const std::vector & traj_points) const -{ - if (prev_optimized_traj_points_ptr_) return *prev_optimized_traj_points_ptr_; - return traj_points; -} - void ElasticBandSmoother::applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points,