diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 43fb310b17416..27f04603d6a31 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -224,18 +225,28 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +static rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + class AEB : public rclcpp::Node { public: explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - rclcpp::Subscription::SharedPtr sub_point_cloud_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_imu_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - + tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", SingleDepthSensorQoS()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + this, "~/input/velocity"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + this, "~/input/predicted_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug @@ -245,15 +256,12 @@ class AEB : public rclcpp::Node // callback void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); - void onVelocity(const VelocityReport::ConstSharedPtr input_msg); void onImu(const Imu::ConstSharedPtr input_msg); void onTimer(); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); - void onAutowareState(const AutowareState::ConstSharedPtr input_msg); rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); - bool isDataReady(); + bool fetchLatestData(); // main function void onCheckCollision(DiagnosticStatusWrapper & stat); diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 1ac255c21921b..68c070a86dd97 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Tomoya Kimura Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index d8886672a8ecd..905b66df288b4 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -105,27 +105,6 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), collision_data_keeper_(this->get_clock()) { - // Subscribers - { - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); - - sub_velocity_ = this->create_subscription( - "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); - - sub_imu_ = this->create_subscription( - "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); - - sub_predicted_traj_ = this->create_subscription( - "~/input/predicted_trajectory", rclcpp::QoS{1}, - std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); - - sub_autoware_state_ = this->create_subscription( - "/autoware/state", rclcpp::QoS{1}, - std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); - } - // Publisher { pub_obstacle_pointcloud_ = @@ -229,11 +208,6 @@ void AEB::onTimer() updater_.force_update(); } -void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) -{ - current_velocity_ptr_ = input_msg; -} - void AEB::onImu(const Imu::ConstSharedPtr input_msg) { // transform imu @@ -253,17 +227,6 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); } -void AEB::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) -{ - predicted_traj_ptr_ = input_msg; -} - -void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) -{ - autoware_state_ = input_msg; -} - void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { PointCloud::Ptr pointcloud_ptr(new PointCloud); @@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) obstacle_ros_pointcloud_ptr_->header = input_msg->header; } -bool AEB::isDataReady() +bool AEB::fetchLatestData() { const auto missing = [this](const auto & name) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); return false; }; + current_velocity_ptr_ = sub_velocity_.takeData(); if (!current_velocity_ptr_) { return missing("ego velocity"); } + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + onPointCloud(pointcloud_ptr); if (!obstacle_ros_pointcloud_ptr_) { return missing("object pointcloud"); } + const auto imu_ptr = sub_imu_.takeData(); + if (use_imu_path_ && !imu_ptr) { + return missing("imu message"); + } + onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); if (use_predicted_trajectory_ && !predicted_traj_ptr_) { return missing("control predicted trajectory"); } + autoware_state_ = sub_autoware_state_.takeData(); if (!autoware_state_) { return missing("autoware_state"); } @@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) using colorTuple = std::tuple; // step1. check data - if (!isDataReady()) { + if (!fetchLatestData()) { return false; }