feat(control): add autonomous emergency braking module (backport auto…

…warefoundation#2793, autowarefoundation#3186, autowarefoundation#3248, autowarefoundation#3292) (#330)

* feat(control): add autonomous emergency braking module (autowarefoundation#2793)

* fix build error

* ci(pre-commit): autofix

* fix: fix lookup symbol error

* fix(autonomous_emergency_braking): fix typo (autowarefoundation#3186)

Update node.cpp

* fix(autonomous_emergency_braking): publish debug marker on base_link frame (autowarefoundation#3248)

* feat(autonomous_emergency_braking): keep collision information for user specified seconds (autowarefoundation#3292)

* feat(autonomous_emergency_braking): keep collision information for user specified seconds

* style(pre-commit): autofix

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

---------

Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

common/tier4_autoware_utils/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/planning/planning_marker_helper.cpp
+  src/geometry/boost_polygon_utils.cpp
 )
 
 # Test

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp

@@ -0,0 +1,34 @@
+// Copyright 2022 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 TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_
+#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_
+
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <boost/geometry.hpp>
+#include <boost/optional.hpp>
+
+#include <vector>
+
+namespace tier4_autoware_utils
+{
+bool isClockwise(const Polygon2d & polygon);
+Polygon2d inverseClockwise(const Polygon2d & polygon);
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -16,6 +16,7 @@
 #define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_
 
 #include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/geometry/pose_deviation.hpp"
 #include "tier4_autoware_utils/math/constants.hpp"

common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp

@@ -0,0 +1,44 @@
+// Copyright 2022 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.
+
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+
+#include <tf2/utils.h>
+
+namespace tier4_autoware_utils
+{
+bool isClockwise(const Polygon2d & polygon)
+{
+  const int n = polygon.outer().size();
+  const double x_offset = polygon.outer().at(0).x();
+  const double y_offset = polygon.outer().at(0).y();
+  double sum = 0.0;
+  for (std::size_t i = 0; i < polygon.outer().size(); ++i) {
+    sum +=
+      (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) -
+      (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset);
+  }
+
+  return sum < 0.0;
+}
+
+Polygon2d inverseClockwise(const Polygon2d & polygon)
+{
+  auto output_polygon = polygon;
+  boost::geometry::reverse(output_polygon);
+  return output_polygon;
+}
+}  // namespace tier4_autoware_utils

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,17 @@
+/**:
+  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
+    collision_keeping_sec: 0.0
+    aeb_hz: 10.0

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -0,0 +1,190 @@
+// 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 <tier4_autoware_utils/tier4_autoware_utils.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
+{
+  rclcpp::Time stamp;
+  geometry_msgs::msg::Point position;
+  double velocity{0.0};
+  double rss{0.0};
+  double distance_to_object{0.0};
+};
+
+class CollisionDataKeeper
+{
+public:
+  explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
+
+  void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; }
+
+  bool checkExpired()
+  {
+    if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) {
+      data_.reset();
+    }
+    return (data_ == nullptr);
+  }
+
+  void update(const ObjectData & data) { data_.reset(new ObjectData(data)); }
+
+  ObjectData get()
+  {
+    if (data_) {
+      return *data_;
+    } else {
+      return ObjectData();
+    }
+  }
+
+private:
+  std::unique_ptr<ObjectData> data_;
+  double timeout_sec_{0.0};
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+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(MarkerArray & debug_markers);
+  bool hasCollision(
+    const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects);
+
+  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, const rclcpp::Time & stamp,
+    std::vector<ObjectData> & objects);
+
+  void addMarker(
+    const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
+    const std::vector<ObjectData> & objects, const double color_r, const double color_g,
+    const double color_b, const double color_a, const std::string & ns,
+    MarkerArray & debug_markers);
+
+  void addCollisionMarker(const ObjectData & data, 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_;
+  CollisionDataKeeper collision_data_keeper_;
+};
+}  // 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,41 @@
+<?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="yutaka.shimizu@tier4.jp">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>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>