Skip to content

Commit

Permalink
feat(path_smoother): make EB's output not std::optional (autowarefoun…
Browse files Browse the repository at this point in the history 
…dation#4371)

* feat(path_smoother): make EB's output not std::optional

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 authored Jul 23, 2023
1 parent ff8c637 commit 8e6c02e
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 91 deletions.
14 changes: 7 additions & 7 deletions planning/path_smoother/include/path_smoother/elastic_band.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TimeKeeper> time_keeper_ptr);

std::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>> getEBTrajectory(
const PlannerData & planner_data);
std::vector<TrajectoryPoint> smoothTrajectory(
const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & ego_pose);

void initialize(const bool enable_debug_info, const CommonParam & common_param);
void resetPreviousData();
Expand Down Expand Up @@ -103,6 +103,7 @@ class EBPathSmoother
EBParam eb_param_;
mutable std::shared_ptr<TimeKeeper> time_keeper_ptr_;
rclcpp::Logger logger_;
rclcpp::Clock clock_;

// publisher
rclcpp::Publisher<Trajectory>::SharedPtr debug_eb_traj_pub_;
Expand All @@ -118,13 +119,12 @@ class EBPathSmoother
const std::vector<TrajectoryPoint> & traj_points) const;

void updateConstraint(
const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points,
const bool is_goal_contained, const int pad_start_idx);
const std::vector<TrajectoryPoint> & traj_points, const bool is_goal_contained,
const int pad_start_idx);

std::optional<std::vector<double>> optimizeTrajectory();
std::optional<std::vector<double>> calcSmoothedTrajectory();

std::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
convertOptimizedPointsToTrajectory(
std::optional<std::vector<TrajectoryPoint>> convertOptimizedPointsToTrajectory(
const std::vector<double> & optimized_points, const std::vector<TrajectoryPoint> & traj_points,
const int pad_start_idx) const;
};
Expand Down
16 changes: 3 additions & 13 deletions planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TrajectoryPoint> generateOptimizedTrajectory(const PlannerData & planner_data);
std::vector<TrajectoryPoint> extendTrajectory(
const std::vector<TrajectoryPoint> & traj_points,
const std::vector<TrajectoryPoint> & optimized_points) const;

// functions in generateOptimizedTrajectory
std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
std::vector<TrajectoryPoint> getPrevOptimizedTrajectory(
const std::vector<TrajectoryPoint> & traj_points) const;
void applyInputVelocity(
std::vector<TrajectoryPoint> & output_traj_points,
const std::vector<TrajectoryPoint> & input_traj_points,
const geometry_msgs::msg::Pose & ego_pose) const;
void insertZeroVelocityOutsideDrivableArea(
const PlannerData & planner_data, std::vector<TrajectoryPoint> & traj_points) const;
void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const;
std::vector<TrajectoryPoint> extendTrajectory(
const std::vector<TrajectoryPoint> & traj_points,
const std::vector<TrajectoryPoint> & optimized_points) const;
};
} // namespace path_smoother

Expand Down
48 changes: 32 additions & 16 deletions planning/path_smoother/src/elastic_band.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,14 @@ std::vector<double> 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
Expand Down Expand Up @@ -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);
Expand All @@ -179,25 +188,30 @@ void EBPathSmoother::resetPreviousData()
prev_eb_traj_points_ptr_ = nullptr;
}

std::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
EBPathSmoother::getEBTrajectory(const PlannerData & planner_data)
std::vector<TrajectoryPoint> EBPathSmoother::smoothTrajectory(
const std::vector<TrajectoryPoint> & 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.
Expand All @@ -221,28 +235,29 @@ 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
const auto eb_traj_points =
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<std::vector<TrajectoryPoint>>(*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__, " ");
Expand Down Expand Up @@ -287,8 +302,8 @@ std::tuple<std::vector<TrajectoryPoint>, size_t> EBPathSmoother::getPaddedTrajec
}

void EBPathSmoother::updateConstraint(
const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points,
const bool is_goal_contained, const int pad_start_idx)
const std::vector<TrajectoryPoint> & traj_points, const bool is_goal_contained,
const int pad_start_idx)
{
time_keeper_ptr_->tic(__func__);

Expand Down Expand Up @@ -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<std::vector<double>> EBPathSmoother::optimizeTrajectory()
std::optional<std::vector<double>> EBPathSmoother::calcSmoothedTrajectory()
{
time_keeper_ptr_->tic(__func__);

Expand Down
65 changes: 10 additions & 55 deletions planning/path_smoother/src/elastic_band_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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<TrajectoryPoint> 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<TrajectoryPoint> 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<TrajectoryPoint> ElasticBandSmoother::getPrevOptimizedTrajectory(
const std::vector<TrajectoryPoint> & traj_points) const
{
if (prev_optimized_traj_points_ptr_) return *prev_optimized_traj_points_ptr_;
return traj_points;
}

void ElasticBandSmoother::applyInputVelocity(
std::vector<TrajectoryPoint> & output_traj_points,
const std::vector<TrajectoryPoint> & input_traj_points,
Expand Down

0 comments on commit 8e6c02e

Please sign in to comment.