Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(path_smoother): use polling subscriber #7214

Merged
merged 1 commit into from
Jun 2, 2024
Merged

feat(path_smoother): use polling subscriber #7214

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 4 additions & 5 deletions planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "path_smoother/type_alias.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

Expand Down Expand Up @@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node
CommonParam common_param_{};
EgoNearestParam ego_nearest_param_{};

// variables for subscribers
Odometry::ConstSharedPtr ego_state_ptr_;

// variables for previous information
std::shared_ptr<std::vector<TrajectoryPoint>> prev_optimized_traj_points_ptr_;

Expand All @@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node

// interface subscriber
rclcpp::Subscription<Path>::SharedPtr path_sub_;
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> odom_sub_{this, "~/input/odometry"};

// debug publisher
rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
Expand All @@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node
void resetPreviousData();

// main functions
bool isDataReady(const Path & path, rclcpp::Clock clock) const;
bool isDataReady(
const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const;
void applyInputVelocity(
std::vector<TrajectoryPoint> & output_traj_points,
const std::vector<TrajectoryPoint> & input_traj_points,
Expand Down
16 changes: 8 additions & 8 deletions planning/path_smoother/src/elastic_band_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option
// interface subscriber
path_sub_ = create_subscription<Path>(
"~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1));
odom_sub_ = create_subscription<Odometry>(
"~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; });

// debug publisher
debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
Expand Down Expand Up @@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
time_keeper_ptr_->tic(__func__);

// check if data is ready and valid
if (!isDataReady(*path_ptr, *get_clock())) {
const auto ego_state_ptr = odom_sub_.takeData();
if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) {
return;
}

Expand All @@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
// 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);
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
Expand All @@ -195,15 +194,15 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
replan_checker_ptr_->updateData(planner_data, is_replan_required, now());
time_keeper_ptr_->tic(__func__);
auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory(
input_traj_points, ego_state_ptr_->pose.pose)
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<std::vector<TrajectoryPoint>>(smoothed_traj_points);

// 2. update velocity
applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose);
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(input_traj_points, smoothed_traj_points);
Expand All @@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp);
}

bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const
bool ElasticBandSmoother::isDataReady(
const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const
{
if (!ego_state_ptr_) {
if (!ego_state_ptr) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist.");
return false;
}
Expand Down
Loading