feat(control): add autonomous emergency braking module (autowarefound…

…ation#2793)

control/autonomous_emergency_braking/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(autonomous_emergency_braking)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+set(AEB_NODE ${PROJECT_NAME}_node)
+ament_auto_add_library(${AEB_NODE} SHARED
+  src/node.cpp
+)
+
+rclcpp_components_register_node(${AEB_NODE}
+  PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+    use_predicted_trajectory: true
+    use_imu_path: true
+    voxel_grid_x: 0.05
+    voxel_grid_y: 0.05
+    voxel_grid_z: 100000.0
+    min_generated_path_length: 0.5
+    expand_width: 0.1
+    longitudinal_offset: 2.0
+    t_response: 1.0
+    a_ego_min: -3.0
+    a_obj_min: -1.0
+    prediction_time_horizon: 1.5
+    prediction_time_interval: 0.1
+    aeb_hz: 10.0

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -0,0 +1,150 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
+#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <pcl_ros/transforms.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
+#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include <boost/optional.hpp>
+
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace autoware::motion::control::autonomous_emergency_braking
+{
+
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_system_msgs::msg::AutowareState;
+using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using sensor_msgs::msg::PointCloud2;
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+using vehicle_info_util::VehicleInfo;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+using Path = std::vector<geometry_msgs::msg::Pose>;
+
+struct ObjectData
+{
+  geometry_msgs::msg::Point position;
+  double velocity;
+};
+
+class AEB : public rclcpp::Node
+{
+public:
+  explicit AEB(const rclcpp::NodeOptions & node_options);
+
+  // subscriber
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_point_cloud_;
+  rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
+  rclcpp::Subscription<Imu>::SharedPtr sub_imu_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
+  rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
+
+  // publisher
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_;  // debug
+
+  // timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // 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);
+
+  bool isDataReady();
+
+  // main function
+  void onCheckCollision(DiagnosticStatusWrapper & stat);
+  bool checkCollision();
+  bool hasCollision(
+    const double current_v, const Path & ego_path, const std::vector<Polygon2d> & ego_polys);
+
+  void generateEgoPath(
+    const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons);
+  void generateEgoPath(
+    const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons);
+  void createObjectData(
+    const Path & ego_path, const std::vector<Polygon2d> & ego_polys,
+    std::vector<ObjectData> & objects);
+
+  void addMarker(
+    const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
+    const double color_r, const double color_g, const double color_b, const double color_a,
+    const std::string & path_ns, const std::string & poly_ns, MarkerArray & debug_markers);
+
+  PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
+  VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
+  Imu::ConstSharedPtr imu_ptr_{nullptr};
+  Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
+  AutowareState::ConstSharedPtr autoware_state_{nullptr};
+
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+
+  // vehicle info
+  VehicleInfo vehicle_info_;
+
+  // diag
+  Updater updater_{this};
+
+  // member variables
+  bool use_predicted_trajectory_;
+  bool use_imu_path_;
+  double voxel_grid_x_;
+  double voxel_grid_y_;
+  double voxel_grid_z_;
+  double min_generated_path_length_;
+  double expand_width_;
+  double longitudinal_offset_;
+  double t_response_;
+  double a_ego_min_;
+  double a_obj_min_;
+  double prediction_time_horizon_;
+  double prediction_time_interval_;
+};
+}  // namespace autoware::motion::control::autonomous_emergency_braking
+
+#endif  // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_

control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml

@@ -0,0 +1,19 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share autonomous_emergency_braking)/config/autonomous_emergency_braking.param.yaml"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <arg name="input_velocity" default="/vehicle/status/velocity_status"/>
+  <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"/>
+
+  <node pkg="autonomous_emergency_braking" exec="autonomous_emergency_braking" name="autonomous_emergency_braking" output="screen">
+    <!-- load config files -->
+    <param from="$(var param_path)"/>
+    <!-- remap topic name -->
+    <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
+    <remap from="~/input/velocity" to="$(var input_velocity)"/>
+    <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)"/>
+  </node>
+</launch>

control/autonomous_emergency_braking/package.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autonomous_emergency_braking</name>
+  <version>0.1.0</version>
+  <description>Autonomous Emergency Braking package as a ROS2 node</description>
+  <maintainer email="[email protected]">yutaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>pointcloud_preprocessor</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>