@@ -105,27 +105,6 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
105
105
vehicle_info_ (vehicle_info_util::VehicleInfoUtil(*this ).getVehicleInfo()),
106
106
collision_data_keeper_(this ->get_clock ())
107
107
{
108
- // Subscribers
109
- {
110
- sub_point_cloud_ = this ->create_subscription <PointCloud2>(
111
- " ~/input/pointcloud" , rclcpp::SensorDataQoS (),
112
- std::bind (&AEB::onPointCloud, this , std::placeholders::_1));
113
-
114
- sub_velocity_ = this ->create_subscription <VelocityReport>(
115
- " ~/input/velocity" , rclcpp::QoS{1 }, std::bind (&AEB::onVelocity, this , std::placeholders::_1));
116
-
117
- sub_imu_ = this ->create_subscription <Imu>(
118
- " ~/input/imu" , rclcpp::QoS{1 }, std::bind (&AEB::onImu, this , std::placeholders::_1));
119
-
120
- sub_predicted_traj_ = this ->create_subscription <Trajectory>(
121
- " ~/input/predicted_trajectory" , rclcpp::QoS{1 },
122
- std::bind (&AEB::onPredictedTrajectory, this , std::placeholders::_1));
123
-
124
- sub_autoware_state_ = this ->create_subscription <AutowareState>(
125
- " /autoware/state" , rclcpp::QoS{1 },
126
- std::bind (&AEB::onAutowareState, this , std::placeholders::_1));
127
- }
128
-
129
108
// Publisher
130
109
{
131
110
pub_obstacle_pointcloud_ =
@@ -229,11 +208,6 @@ void AEB::onTimer()
229
208
updater_.force_update ();
230
209
}
231
210
232
- void AEB::onVelocity (const VelocityReport::ConstSharedPtr input_msg)
233
- {
234
- current_velocity_ptr_ = input_msg;
235
- }
236
-
237
211
void AEB::onImu (const Imu::ConstSharedPtr input_msg)
238
212
{
239
213
// transform imu
@@ -253,17 +227,6 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg)
253
227
tf2::doTransform (input_msg->angular_velocity , *angular_velocity_ptr_, transform_stamped);
254
228
}
255
229
256
- void AEB::onPredictedTrajectory (
257
- const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
258
- {
259
- predicted_traj_ptr_ = input_msg;
260
- }
261
-
262
- void AEB::onAutowareState (const AutowareState::ConstSharedPtr input_msg)
263
- {
264
- autoware_state_ = input_msg;
265
- }
266
-
267
230
void AEB::onPointCloud (const PointCloud2::ConstSharedPtr input_msg)
268
231
{
269
232
PointCloud::Ptr pointcloud_ptr (new PointCloud);
@@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
316
279
obstacle_ros_pointcloud_ptr_->header = input_msg->header ;
317
280
}
318
281
319
- bool AEB::isDataReady ()
282
+ bool AEB::fetchLatestData ()
320
283
{
321
284
const auto missing = [this ](const auto & name) {
322
285
RCLCPP_INFO_SKIPFIRST_THROTTLE (get_logger (), *get_clock (), 5000 , " [AEB] waiting for %s" , name);
323
286
return false ;
324
287
};
325
288
289
+ current_velocity_ptr_ = sub_velocity_.takeData ();
326
290
if (!current_velocity_ptr_) {
327
291
return missing (" ego velocity" );
328
292
}
329
293
294
+ const auto pointcloud_ptr = sub_point_cloud_.takeData ();
295
+ if (!pointcloud_ptr) {
296
+ return missing (" object pointcloud message" );
297
+ }
298
+ onPointCloud (pointcloud_ptr);
330
299
if (!obstacle_ros_pointcloud_ptr_) {
331
300
return missing (" object pointcloud" );
332
301
}
333
302
303
+ const auto imu_ptr = sub_imu_.takeData ();
304
+ if (use_imu_path_ && !imu_ptr) {
305
+ return missing (" imu message" );
306
+ }
307
+ onImu (imu_ptr);
334
308
if (use_imu_path_ && !angular_velocity_ptr_) {
335
309
return missing (" imu" );
336
310
}
337
311
312
+ predicted_traj_ptr_ = sub_predicted_traj_.takeData ();
338
313
if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
339
314
return missing (" control predicted trajectory" );
340
315
}
341
316
317
+ autoware_state_ = sub_autoware_state_.takeData ();
342
318
if (!autoware_state_) {
343
319
return missing (" autoware_state" );
344
320
}
@@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
375
351
using colorTuple = std::tuple<double , double , double , double >;
376
352
377
353
// step1. check data
378
- if (!isDataReady ()) {
354
+ if (!fetchLatestData ()) {
379
355
return false ;
380
356
}
381
357
0 commit comments