Skip to content

Commit 1a01375

Browse files
committed
replace polling subscriber
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 271e6f1 commit 1a01375

File tree

2 files changed

+40
-31
lines changed
  • control/autonomous_emergency_braking

2 files changed

+40
-31
lines changed

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+10-10
Original file line numberDiff line numberDiff line change
@@ -20,7 +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_subsriber.hpp>
23+
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
2424
#include <vehicle_info_util/vehicle_info_util.hpp>
2525

2626
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
@@ -240,7 +240,7 @@ class AEB : public rclcpp::Node
240240
// subscriber
241241
tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
242242
this, "~/input/pointcloud", getSingleDepthSensorQoS()};
243-
tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_veloclty_{
243+
tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
244244
this, "~/input/velocity"};
245245
tier4_autoware_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
246246
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
@@ -256,12 +256,12 @@ class AEB : public rclcpp::Node
256256
rclcpp::TimerBase::SharedPtr timer_;
257257

258258
// callback
259-
void onPointCloud(const PointCloud2::ConstSharedPtr input_msg);
260-
void onVelocity(const VelocityReport::ConstSharedPtr input_msg);
261-
void onImu(const Imu::ConstSharedPtr input_msg);
259+
void fetchPointCloud();
260+
void fetchVelocity();
261+
void fetchImu();
262+
void fetchPredictedTrajectory();
263+
void fetchAutowareState();
262264
void onTimer();
263-
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
264-
void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
265265
rcl_interfaces::msg::SetParametersResult onParameter(
266266
const std::vector<rclcpp::Parameter> & parameters);
267267

@@ -301,10 +301,10 @@ class AEB : public rclcpp::Node
301301
const ObjectData & closest_object, const Path & path, const double current_ego_speed);
302302

303303
PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
304-
VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
304+
VelocityReport::SharedPtr current_velocity_ptr_{nullptr};
305305
Vector3::SharedPtr angular_velocity_ptr_{nullptr};
306-
Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
307-
AutowareState::ConstSharedPtr autoware_state_{nullptr};
306+
Trajectory::SharedPtr predicted_traj_ptr_{nullptr};
307+
AutowareState::SharedPtr autoware_state_{nullptr};
308308

309309
tf2_ros::Buffer tf_buffer_{get_clock()};
310310
tf2_ros::TransformListener tf_listener_{tf_buffer_};

control/autonomous_emergency_braking/src/node.cpp

+30-21
Original file line numberDiff line numberDiff line change
@@ -208,68 +208,72 @@ void AEB::onTimer()
208208
updater_.force_update();
209209
}
210210

211-
void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
211+
void AEB::fetchVelocity()
212212
{
213-
current_velocity_ptr_ = input_msg;
213+
current_velocity_ptr_ = std::make_shared<VelocityReport>();
214+
*current_velocity_ptr_ = sub_velocity_.getData();
214215
}
215216

216-
void AEB::onImu(const Imu::ConstSharedPtr input_msg)
217+
void AEB::fetchImu()
217218
{
219+
const auto & input_msg = sub_imu_.getData();
218220
// transform imu
219221
geometry_msgs::msg::TransformStamped transform_stamped{};
220222
try {
221223
transform_stamped = tf_buffer_.lookupTransform(
222-
"base_link", input_msg->header.frame_id, input_msg->header.stamp,
224+
"base_link", input_msg.header.frame_id, input_msg.header.stamp,
223225
rclcpp::Duration::from_seconds(0.5));
224226
} catch (tf2::TransformException & ex) {
225227
RCLCPP_ERROR_STREAM(
226228
get_logger(),
227-
"[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
229+
"[AEB] Failed to look up transform from base_link to" << input_msg.header.frame_id);
228230
return;
229231
}
230232

231233
angular_velocity_ptr_ = std::make_shared<Vector3>();
232-
tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
234+
tf2::doTransform(input_msg.angular_velocity, *angular_velocity_ptr_, transform_stamped);
233235
}
234236

235-
void AEB::onPredictedTrajectory(
236-
const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
237+
void AEB::fetchPredictedTrajectory()
237238
{
238-
predicted_traj_ptr_ = input_msg;
239+
predicted_traj_ptr_ = std::make_shared<Trajectory>();
240+
*predicted_traj_ptr_ = sub_predicted_traj_.getData();
239241
}
240242

241-
void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg)
243+
void AEB::fetchAutowareState()
242244
{
243-
autoware_state_ = input_msg;
245+
autoware_state_ = std::make_shared<AutowareState>();
246+
*autoware_state_ = sub_autoware_state_.getData();
244247
}
245248

246-
void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
249+
void AEB::fetchPointCloud()
247250
{
251+
const auto & input_msg = sub_point_cloud_.getData();
248252
PointCloud::Ptr pointcloud_ptr(new PointCloud);
249-
pcl::fromROSMsg(*input_msg, *pointcloud_ptr);
253+
pcl::fromROSMsg(input_msg, *pointcloud_ptr);
250254

251-
if (input_msg->header.frame_id != "base_link") {
255+
if (input_msg.header.frame_id != "base_link") {
252256
RCLCPP_ERROR_STREAM(
253257
get_logger(),
254-
"[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id);
258+
"[AEB]: Input point cloud frame is not base_link and it is " << input_msg.header.frame_id);
255259
// transform pointcloud
256260
geometry_msgs::msg::TransformStamped transform_stamped{};
257261
try {
258262
transform_stamped = tf_buffer_.lookupTransform(
259-
"base_link", input_msg->header.frame_id, input_msg->header.stamp,
263+
"base_link", input_msg.header.frame_id, input_msg.header.stamp,
260264
rclcpp::Duration::from_seconds(0.5));
261265
} catch (tf2::TransformException & ex) {
262266
RCLCPP_ERROR_STREAM(
263267
get_logger(),
264-
"[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
268+
"[AEB] Failed to look up transform from base_link to" << input_msg.header.frame_id);
265269
return;
266270
}
267271

268272
// transform by using eigen matrix
269273
PointCloud2 transformed_points{};
270274
const Eigen::Matrix4f affine_matrix =
271275
tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
272-
pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points);
276+
pcl_ros::transformPointCloud(affine_matrix, input_msg, transformed_points);
273277
pcl::fromROSMsg(transformed_points, *pointcloud_ptr);
274278
}
275279

@@ -292,7 +296,7 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
292296
obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
293297

294298
pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
295-
obstacle_ros_pointcloud_ptr_->header = input_msg->header;
299+
obstacle_ros_pointcloud_ptr_->header = input_msg.header;
296300
}
297301

298302
bool AEB::isDataReady()
@@ -302,22 +306,27 @@ bool AEB::isDataReady()
302306
return false;
303307
};
304308

309+
fetchVelocity();
305310
if (!current_velocity_ptr_) {
306311
return missing("ego velocity");
307312
}
308313

314+
fetchPointCloud();
309315
if (!obstacle_ros_pointcloud_ptr_) {
310-
return missing("object pointcloud");
316+
return missing("filtered object pointcloud");
311317
}
312318

319+
fetchImu();
313320
if (use_imu_path_ && !angular_velocity_ptr_) {
314-
return missing("imu");
321+
return missing("filtered imu");
315322
}
316323

324+
fetchPredictedTrajectory();
317325
if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
318326
return missing("control predicted trajectory");
319327
}
320328

329+
fetchAutowareState();
321330
if (!autoware_state_) {
322331
return missing("autoware_state");
323332
}

0 commit comments

Comments
 (0)