add polling sub to predicted objects

Signed-off-by: Daniel Sanchez <[email protected]>

control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp

@@ -23,6 +23,7 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <autoware_vehicle_msgs/msg/velocity_report.hpp>
@@ -68,6 +69,7 @@ using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
 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<Imu> sub_imu_{this, "~/input/imu"};
   tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_traj_{
     this, "~/input/predicted_trajectory"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> predicted_objects_sub_{
+    this, "~/input/objects"};
   tier4_autoware_utils::InterProcessPollingSubscriber<AutowareState> 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()};

control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml

@@ -5,6 +5,7 @@
   <arg name="input_imu" default="/sensing/imu/imu_data"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_predicted_trajectory" default="/control/trajectory_follower/lateral/predicted_trajectory"/>
+  <arg name="input_objects" default="/perception/object_recognition/objects"/>
 
   <node pkg="autoware_autonomous_emergency_braking" exec="autoware_autonomous_emergency_braking" name="autonomous_emergency_braking" output="screen">
     <!-- load config files -->
@@ -15,5 +16,6 @@
     <remap from="~/input/imu" to="$(var input_imu)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/predicted_trajectory" to="$(var input_predicted_trajectory)"/>
+    <remap from="~/input/objects" to="$(var input_objects)"/>
   </node>
 </launch>

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");

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",