diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp index d3ac808616b50..2c12cb4314cb5 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,7 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObjects; struct ObjectData { rclcpp::Time stamp; @@ -249,6 +251,8 @@ class AEB : public rclcpp::Node tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + this, "~/input/objects"}; tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher @@ -304,6 +308,7 @@ class AEB : public rclcpp::Node VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; tf2_ros::Buffer tf_buffer_{get_clock()}; diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index 75b1a9dcf822d..fe0df41ca89c7 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -5,6 +5,7 @@ + @@ -15,5 +16,6 @@ + diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 945d8508cbc05..cb30752e96753 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -320,6 +320,11 @@ bool AEB::fetchLatestData() return missing("control predicted trajectory"); } + predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + if (!predicted_objects_ptr_) { + return missing("predicted objects"); + } + autoware_state_ = sub_autoware_state_.takeData(); if (!autoware_state_) { return missing("autoware_state"); diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c128a5afbf34e..97f90524eb505 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -148,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/velocity", "/vehicle/status/velocity_status"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), ( "~/input/predicted_trajectory", "/control/trajectory_follower/lateral/predicted_trajectory",