Skip to content

Commit

Permalink
feat(path_smoother): use polling subscriber (#7214)
Browse files Browse the repository at this point in the history
Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
takayuki5168 authored Jun 2, 2024
1 parent d70c8c9 commit a553c6d
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 13 deletions.
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
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

0 comments on commit a553c6d

Please sign in to comment.