Skip to content

Commit 74da4c1

Browse files
authored
feat(aeb): replace InterProcessPollingSubscriber (#6997)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 5105e83 commit 74da4c1

File tree

3 files changed

+34
-49
lines changed

3 files changed

+34
-49
lines changed

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+18-10
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <pcl_ros/transforms.hpp>
2121
#include <rclcpp/rclcpp.hpp>
2222
#include <tier4_autoware_utils/geometry/geometry.hpp>
23+
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
2324
#include <vehicle_info_util/vehicle_info_util.hpp>
2425

2526
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
@@ -224,18 +225,28 @@ class CollisionDataKeeper
224225
rclcpp::Clock::SharedPtr clock_;
225226
};
226227

228+
static rclcpp::SensorDataQoS SingleDepthSensorQoS()
229+
{
230+
rclcpp::SensorDataQoS qos;
231+
qos.get_rmw_qos_profile().depth = 1;
232+
return qos;
233+
}
234+
227235
class AEB : public rclcpp::Node
228236
{
229237
public:
230238
explicit AEB(const rclcpp::NodeOptions & node_options);
231239

232240
// subscriber
233-
rclcpp::Subscription<PointCloud2>::SharedPtr sub_point_cloud_;
234-
rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
235-
rclcpp::Subscription<Imu>::SharedPtr sub_imu_;
236-
rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
237-
rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
238-
241+
tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
242+
this, "~/input/pointcloud", SingleDepthSensorQoS()};
243+
tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
244+
this, "~/input/velocity"};
245+
tier4_autoware_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
246+
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
247+
this, "~/input/predicted_trajectory"};
248+
tier4_autoware_utils::InterProcessPollingSubscriber<AutowareState> sub_autoware_state_{
249+
this, "/autoware/state"};
239250
// publisher
240251
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
241252
rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_; // debug
@@ -245,15 +256,12 @@ class AEB : public rclcpp::Node
245256

246257
// callback
247258
void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
248-
void onVelocity(const VelocityReport::ConstSharedPtr input_msg);
249259
void onImu(const Imu::ConstSharedPtr input_msg);
250260
void onTimer();
251-
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
252-
void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
253261
rcl_interfaces::msg::SetParametersResult onParameter(
254262
const std::vector<rclcpp::Parameter> & parameters);
255263

256-
bool isDataReady();
264+
bool fetchLatestData();
257265

258266
// main function
259267
void onCheckCollision(DiagnosticStatusWrapper & stat);

control/autonomous_emergency_braking/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
88
<maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
99
<maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
10+
<maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
1011

1112
<license>Apache License 2.0</license>
1213

control/autonomous_emergency_braking/src/node.cpp

+15-39
Original file line numberDiff line numberDiff line change
@@ -105,27 +105,6 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
105105
vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()),
106106
collision_data_keeper_(this->get_clock())
107107
{
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-
129108
// Publisher
130109
{
131110
pub_obstacle_pointcloud_ =
@@ -229,11 +208,6 @@ void AEB::onTimer()
229208
updater_.force_update();
230209
}
231210

232-
void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
233-
{
234-
current_velocity_ptr_ = input_msg;
235-
}
236-
237211
void AEB::onImu(const Imu::ConstSharedPtr input_msg)
238212
{
239213
// transform imu
@@ -253,17 +227,6 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg)
253227
tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
254228
}
255229

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-
267230
void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
268231
{
269232
PointCloud::Ptr pointcloud_ptr(new PointCloud);
@@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
316279
obstacle_ros_pointcloud_ptr_->header = input_msg->header;
317280
}
318281

319-
bool AEB::isDataReady()
282+
bool AEB::fetchLatestData()
320283
{
321284
const auto missing = [this](const auto & name) {
322285
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name);
323286
return false;
324287
};
325288

289+
current_velocity_ptr_ = sub_velocity_.takeData();
326290
if (!current_velocity_ptr_) {
327291
return missing("ego velocity");
328292
}
329293

294+
const auto pointcloud_ptr = sub_point_cloud_.takeData();
295+
if (!pointcloud_ptr) {
296+
return missing("object pointcloud message");
297+
}
298+
onPointCloud(pointcloud_ptr);
330299
if (!obstacle_ros_pointcloud_ptr_) {
331300
return missing("object pointcloud");
332301
}
333302

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);
334308
if (use_imu_path_ && !angular_velocity_ptr_) {
335309
return missing("imu");
336310
}
337311

312+
predicted_traj_ptr_ = sub_predicted_traj_.takeData();
338313
if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
339314
return missing("control predicted trajectory");
340315
}
341316

317+
autoware_state_ = sub_autoware_state_.takeData();
342318
if (!autoware_state_) {
343319
return missing("autoware_state");
344320
}
@@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
375351
using colorTuple = std::tuple<double, double, double, double>;
376352

377353
// step1. check data
378-
if (!isDataReady()) {
354+
if (!fetchLatestData()) {
379355
return false;
380356
}
381357

0 commit comments

Comments
 (0)