Skip to content

Commit e3a30cd

Browse files
authored
feat(sampling_based_planner): use polling subscribers (autowarefoundation#7394)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 2f3541b commit e3a30cd

File tree

2 files changed

+23
-22
lines changed
  • planning/sampling_based_planner/autoware_path_sampler

2 files changed

+23
-22
lines changed

planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp

+7-5
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "rclcpp/rclcpp.hpp"
2424

2525
#include <autoware_sampler_common/structures.hpp>
26+
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
2627

2728
#include <algorithm>
2829
#include <memory>
@@ -51,7 +52,6 @@ class PathSampler : public rclcpp::Node
5152

5253
// variables for subscribers
5354
Odometry::SharedPtr ego_state_ptr_;
54-
PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared<PredictedObjects>();
5555

5656
// variables for previous information
5757
std::optional<autoware::sampler_common::Path> prev_path_;
@@ -62,8 +62,9 @@ class PathSampler : public rclcpp::Node
6262

6363
// interface subscriber
6464
rclcpp::Subscription<Path>::SharedPtr path_sub_;
65-
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
66-
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
65+
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> odom_sub_{this, "~/input/odometry"};
66+
tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
67+
this, "~/input/objects"};
6768

6869
// debug publisher
6970
rclcpp::Publisher<MarkerArray>::SharedPtr debug_markers_pub_;
@@ -79,8 +80,9 @@ class PathSampler : public rclcpp::Node
7980
void onPath(const Path::SharedPtr);
8081

8182
// main functions
82-
bool isDataReady(const Path & path, rclcpp::Clock clock) const;
83-
PlannerData createPlannerData(const Path & path) const;
83+
bool isDataReady(
84+
const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock);
85+
PlannerData createPlannerData(const Path & path, const Odometry & ego_state) const;
8486
std::vector<TrajectoryPoint> generateTrajectory(const PlannerData & planner_data);
8587
std::vector<TrajectoryPoint> extendTrajectory(
8688
const std::vector<TrajectoryPoint> & traj_points,

planning/sampling_based_planner/autoware_path_sampler/src/node.cpp

+16-17
Original file line numberDiff line numberDiff line change
@@ -70,10 +70,6 @@ PathSampler::PathSampler(const rclcpp::NodeOptions & node_options)
7070
// interface subscriber
7171
path_sub_ = create_subscription<Path>(
7272
"~/input/path", 1, std::bind(&PathSampler::onPath, this, std::placeholders::_1));
73-
odom_sub_ = create_subscription<Odometry>(
74-
"~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; });
75-
objects_sub_ = create_subscription<PredictedObjects>(
76-
"~/input/objects", 1, std::bind(&PathSampler::objectsCallback, this, std::placeholders::_1));
7773

7874
// debug publisher
7975
debug_markers_pub_ = create_publisher<MarkerArray>("~/debug/marker", 1);
@@ -215,11 +211,6 @@ void PathSampler::resetPreviousData()
215211
prev_path_.reset();
216212
}
217213

218-
void PathSampler::objectsCallback(const PredictedObjects::SharedPtr msg)
219-
{
220-
in_objects_ptr_ = msg;
221-
}
222-
223214
autoware::sampler_common::State PathSampler::getPlanningState(
224215
autoware::sampler_common::State & state,
225216
const autoware::sampler_common::transform::Spline2D & path_spline) const
@@ -241,7 +232,8 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
241232
time_keeper_ptr_->tic(__func__);
242233

243234
// check if data is ready and valid
244-
if (!isDataReady(*path_ptr, *get_clock())) {
235+
const auto ego_state_ptr = odom_sub_.takeData();
236+
if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) {
245237
return;
246238
}
247239

@@ -253,7 +245,7 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
253245
"Backward path is NOT supported. Just converting path to trajectory");
254246
}
255247
// 1. create planner data
256-
const auto planner_data = createPlannerData(*path_ptr);
248+
const auto planner_data = createPlannerData(*path_ptr, *ego_state_ptr);
257249
// 2. generate trajectory
258250
const auto generated_traj_points = generateTrajectory(planner_data);
259251
// 3. extend trajectory to connect the optimized trajectory and the following path smoothly
@@ -278,9 +270,10 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
278270
debug_calculation_time_pub_->publish(calculation_time_msg);
279271
}
280272

281-
bool PathSampler::isDataReady(const Path & path, rclcpp::Clock clock) const
273+
bool PathSampler::isDataReady(
274+
const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock)
282275
{
283-
if (!ego_state_ptr_) {
276+
if (!ego_state_ptr) {
284277
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist.");
285278
return false;
286279
}
@@ -296,19 +289,24 @@ bool PathSampler::isDataReady(const Path & path, rclcpp::Clock clock) const
296289
return false;
297290
}
298291

292+
if (!objects_sub_.takeData()) {
293+
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for detected objects.");
294+
return false;
295+
}
296+
299297
return true;
300298
}
301299

302-
PlannerData PathSampler::createPlannerData(const Path & path) const
300+
PlannerData PathSampler::createPlannerData(const Path & path, const Odometry & ego_state) const
303301
{
304302
// create planner data
305303
PlannerData planner_data;
306304
planner_data.header = path.header;
307305
planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points);
308306
planner_data.left_bound = path.left_bound;
309307
planner_data.right_bound = path.right_bound;
310-
planner_data.ego_pose = ego_state_ptr_->pose.pose;
311-
planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
308+
planner_data.ego_pose = ego_state.pose.pose;
309+
planner_data.ego_vel = ego_state.twist.twist.linear.x;
312310
return planner_data;
313311
}
314312

@@ -467,8 +465,9 @@ autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & pla
467465
current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation);
468466

469467
const auto planning_state = getPlanningState(current_state, path_spline);
468+
const auto objects = objects_sub_.takeData();
470469
prepareConstraints(
471-
params_.constraints, *in_objects_ptr_, planner_data.left_bound, planner_data.right_bound);
470+
params_.constraints, *objects, planner_data.left_bound, planner_data.right_bound);
472471

473472
auto candidate_paths = generateCandidatePaths(planning_state, path_spline, 0.0, params_);
474473
const auto candidates_from_prev_path =

0 commit comments

Comments
 (0)