@@ -70,10 +70,6 @@ PathSampler::PathSampler(const rclcpp::NodeOptions & node_options)
70
70
// interface subscriber
71
71
path_sub_ = create_subscription<Path>(
72
72
" ~/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));
77
73
78
74
// debug publisher
79
75
debug_markers_pub_ = create_publisher<MarkerArray>(" ~/debug/marker" , 1 );
@@ -215,11 +211,6 @@ void PathSampler::resetPreviousData()
215
211
prev_path_.reset ();
216
212
}
217
213
218
- void PathSampler::objectsCallback (const PredictedObjects::SharedPtr msg)
219
- {
220
- in_objects_ptr_ = msg;
221
- }
222
-
223
214
autoware::sampler_common::State PathSampler::getPlanningState (
224
215
autoware::sampler_common::State & state,
225
216
const autoware::sampler_common::transform::Spline2D & path_spline) const
@@ -241,7 +232,8 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
241
232
time_keeper_ptr_->tic (__func__);
242
233
243
234
// 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 ())) {
245
237
return ;
246
238
}
247
239
@@ -253,7 +245,7 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
253
245
" Backward path is NOT supported. Just converting path to trajectory" );
254
246
}
255
247
// 1. create planner data
256
- const auto planner_data = createPlannerData (*path_ptr);
248
+ const auto planner_data = createPlannerData (*path_ptr, *ego_state_ptr );
257
249
// 2. generate trajectory
258
250
const auto generated_traj_points = generateTrajectory (planner_data);
259
251
// 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)
278
270
debug_calculation_time_pub_->publish (calculation_time_msg);
279
271
}
280
272
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)
282
275
{
283
- if (!ego_state_ptr_ ) {
276
+ if (!ego_state_ptr ) {
284
277
RCLCPP_INFO_SKIPFIRST_THROTTLE (get_logger (), clock , 5000 , " Waiting for ego pose and twist." );
285
278
return false ;
286
279
}
@@ -296,19 +289,24 @@ bool PathSampler::isDataReady(const Path & path, rclcpp::Clock clock) const
296
289
return false ;
297
290
}
298
291
292
+ if (!objects_sub_.takeData ()) {
293
+ RCLCPP_INFO_SKIPFIRST_THROTTLE (get_logger (), clock , 5000 , " Waiting for detected objects." );
294
+ return false ;
295
+ }
296
+
299
297
return true ;
300
298
}
301
299
302
- PlannerData PathSampler::createPlannerData (const Path & path) const
300
+ PlannerData PathSampler::createPlannerData (const Path & path, const Odometry & ego_state ) const
303
301
{
304
302
// create planner data
305
303
PlannerData planner_data;
306
304
planner_data.header = path.header ;
307
305
planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints (path.points );
308
306
planner_data.left_bound = path.left_bound ;
309
307
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 ;
312
310
return planner_data;
313
311
}
314
312
@@ -467,8 +465,9 @@ autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & pla
467
465
current_state.heading = tf2::getYaw (planner_data.ego_pose .orientation );
468
466
469
467
const auto planning_state = getPlanningState (current_state, path_spline);
468
+ const auto objects = objects_sub_.takeData ();
470
469
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 );
472
471
473
472
auto candidate_paths = generateCandidatePaths (planning_state, path_spline, 0.0 , params_);
474
473
const auto candidates_from_prev_path =
0 commit comments