diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt new file mode 100644 index 0000000000000..6027aeac76333 --- /dev/null +++ b/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 +) diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml new file mode 100644 index 0000000000000..6019eaae503a0 --- /dev/null +++ b/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 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp new file mode 100644 index 0000000000000..64a3079df106a --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +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; +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; + +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::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_; + + // publisher + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + rclcpp::Publisher::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 & ego_polys); + + void generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons); + void generateEgoPath( + const Trajectory & predicted_traj, Path & path, std::vector & polygons); + void createObjectData( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + + void addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & 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_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml new file mode 100644 index 0000000000000..cf6640ec6be52 --- /dev/null +++ b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml new file mode 100644 index 0000000000000..198afb1018e31 --- /dev/null +++ b/control/autonomous_emergency_braking/package.xml @@ -0,0 +1,42 @@ + + + + autonomous_emergency_braking + 0.1.0 + Autonomous Emergency Braking package as a ROS2 node + yutaka + Apache License 2.0 + + ament_cmake + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + pcl_conversions + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp new file mode 100644 index 0000000000000..77ea7e6318820 --- /dev/null +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -0,0 +1,495 @@ +// 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 "autonomous_emergency_braking/node.hpp" + +#include +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace autoware::motion::control::autonomous_emergency_braking +{ +using diagnostic_msgs::msg::DiagnosticStatus; +namespace bg = boost::geometry; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +Polygon2d createPolygon( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + +AEB::AEB(const rclcpp::NodeOptions & node_options) +: Node("AEB", node_options), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +{ + // 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_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + + // Diagnostics + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + + // parameter + use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); + use_imu_path_ = declare_parameter("use_imu_path"); + voxel_grid_x_ = declare_parameter("voxel_grid_x"); + voxel_grid_y_ = declare_parameter("voxel_grid_y"); + voxel_grid_z_ = declare_parameter("voxel_grid_z"); + min_generated_path_length_ = declare_parameter("min_generated_path_length"); + expand_width_ = declare_parameter("expand_width"); + longitudinal_offset_ = declare_parameter("longitudinal_offset"); + t_response_ = declare_parameter("t_response"); + a_ego_min_ = declare_parameter("a_ego_min"); + a_obj_min_ = declare_parameter("a_obj_min"); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_interval_ = declare_parameter("prediction_time_interval"); + + // start time + const double aeb_hz = declare_parameter("aeb_hz"); + const auto period_ns = rclcpp::Rate(aeb_hz).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&AEB::onTimer, this)); +} + +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) { imu_ptr_ = input_msg; } + +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); + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + if (input_msg->header.frame_id != "base_link") { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); + pcl::fromROSMsg(transformed_points, *pointcloud_ptr); + } + + pcl::VoxelGrid filter; + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); + filter.filter(*no_height_filtered_pointcloud_ptr); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); +} + +bool AEB::isDataReady() +{ + const auto missing = [this](const auto & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); + return false; + }; + + if (!current_velocity_ptr_) { + return missing("ego velocity"); + } + + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + + if (use_imu_path_ && !imu_ptr_) { + return missing("object pointcloud"); + } + + if (use_predicted_trajectory_ && !predicted_traj_ptr_) { + return missing("control predicted trajectory"); + } + + if (!autoware_state_) { + return missing("autoware_state"); + } + + return true; +} + +void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) +{ + if (checkCollision()) { + const std::string error_msg = "[AEB]: Emergency Brake"; + const auto diag_level = DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + return; + } + + const std::string error_msg = "[AEB]: No Collision"; + const auto diag_level = DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); +} + +bool AEB::checkCollision() +{ + // step1. check data + if (!isDataReady()) { + return false; + } + + // if not driving, disable aeb + if (autoware_state_->state != AutowareState::DRIVING) { + return false; + } + + // step2. create velocity data check if the vehicle stops or not + const double current_v = current_velocity_ptr_->longitudinal_velocity; + if (current_v < 0.1) { + return false; + } + + MarkerArray debug_markers; + + // step3. create ego path based on sensor data + Path ego_path; + std::vector ego_polys; + if (use_imu_path_) { + const double current_w = imu_ptr_->angular_velocity.z; + constexpr double color_r = 0.0 / 256.0; + constexpr double color_g = 148.0 / 256.0; + constexpr double color_b = 205.0 / 256.0; + constexpr double color_a = 0.999; + const auto current_time = get_clock()->now(); + generateEgoPath(current_v, current_w, ego_path, ego_polys); + addMarker( + current_time, ego_path, ego_polys, color_r, color_g, color_b, color_a, "ego_path", + "ego_polygons", debug_markers); + } + + // step4. transform predicted trajectory from control module + Path predicted_path; + std::vector predicted_polys; + if (use_predicted_trajectory_) { + const auto predicted_traj_ptr = predicted_traj_ptr_; + constexpr double color_r = 0.0; + constexpr double color_g = 100.0 / 256.0; + constexpr double color_b = 0.0; + constexpr double color_a = 0.999; + const auto current_time = predicted_traj_ptr->header.stamp; + generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys); + addMarker( + current_time, predicted_path, predicted_polys, color_r, color_g, color_b, color_a, + "predicted_path", "predicted_polygons", debug_markers); + } + + // publish debug markers + debug_ego_path_publisher_->publish(debug_markers); + + return hasCollision(current_v, ego_path, ego_polys) || + hasCollision(current_v, predicted_path, predicted_polys); +} + +bool AEB::hasCollision( + const double current_v, const Path & ego_path, const std::vector & ego_polys) +{ + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty()) { + return false; + } + + // step1. create object + std::vector objects; + createObjectData(ego_path, ego_polys, objects); + + // step2. calculate RSS + const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const double & t = t_response_; + for (const auto & obj : objects) { + const double & obj_v = obj.velocity; + const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + return true; + } + } + + return false; +} + +void AEB::generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons) +{ + double curr_x = 0.0; + double curr_y = 0.0; + double curr_yaw = 0.0; + geometry_msgs::msg::Pose ini_pose; + ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + path.push_back(ini_pose); + + if (curr_v < 0.1) { + // if current velocity is too small, assume it stops at the same point + return; + } + + constexpr double epsilon = 1e-6; + const double & dt = prediction_time_interval_; + const double & horizon = prediction_time_horizon_; + for (double t = 0.0; t < horizon + epsilon; t += dt) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // If path is shorter than minimum path length + while (motion_utils::calcArcLength(path) < min_generated_path_length_) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // generate ego polygons + polygons.resize(path.size() - 1); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::generateEgoPath( + const Trajectory & predicted_traj, Path & path, + std::vector & polygons) +{ + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + // create path + path.resize(predicted_traj.points.size()); + for (size_t i = 0; i < predicted_traj.points.size(); ++i) { + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + path.at(i) = map_pose; + } + + // create polygon + polygons.resize(path.size()); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::createObjectData( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects) +{ + PointCloud::Ptr obstacle_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_points_ptr); + for (const auto & point : obstacle_points_ptr->points) { + ObjectData obj; + obj.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + obj.velocity = 0.0; + const Point2d obj_point(point.x, point.y); + const double lat_dist = motion_utils::calcLateralOffset(ego_path, obj.position); + if (lat_dist > 5.0) { + continue; + } + for (const auto & ego_poly : ego_polys) { + if (bg::within(obj_point, ego_poly)) { + objects.push_back(obj); + break; + } + } + } +} + +void AEB::addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & 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) +{ + // transform to map + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "map", "base_link", current_time, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + auto path_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, path_ns, 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + path_marker.points.resize(path.size()); + for (size_t i = 0; i < path.size(); ++i) { + const auto & pose = path.at(i); + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(pose, map_pose, transform_stamped); + path_marker.points.at(i) = map_pose.position; + } + debug_markers.markers.push_back(path_marker); + + auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + "map", current_time, poly_ns, 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + + geometry_msgs::msg::Point map_curr_point; + geometry_msgs::msg::Point map_next_point; + tf2::doTransform(curr_point, map_curr_point, transform_stamped); + tf2::doTransform(next_point, map_next_point, transform_stamped); + polygon_marker.points.push_back(map_curr_point); + polygon_marker.points.push_back(map_next_point); + } + } + debug_markers.markers.push_back(polygon_marker); +} +} // namespace autoware::motion::control::autonomous_emergency_braking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 593a79cd2b361..6821388e174d4 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs): LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" ) as f: obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: + aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -126,6 +128,33 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # autonomous emergency braking + autonomous_emergency_braking = ComposableNode( + package="autonomous_emergency_braking", + plugin="autoware::motion::control::autonomous_emergency_braking::AEB", + name="autonomous_emergency_braking", + remappings=[ + ("~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/velocity", "/vehicle/status/velocity_status"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + aeb_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + autonomous_emergency_braking_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_autonomous_emergency_braking")), + composable_node_descriptions=[autonomous_emergency_braking], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -274,6 +303,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_loader, external_cmd_converter_loader, obstacle_collision_checker_loader, + autonomous_emergency_braking_loader, ] ) @@ -306,6 +336,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("shift_decider_param_path") add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") + add_launch_arg("aeb_param_path") + add_launch_arg("enable_autonomous_emergency_braking") # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml index 360b15a9d7e45..86d84bba7c41c 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml @@ -4,6 +4,20 @@ type: diagnostic_aggregator/AnalyzerGroup path: control analyzers: + autonomous_emergency_braking: + type: diagnostic_aggregator/AnalyzerGroup + path: autonomous_emergency_braking + analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + emergency_stop: + type: diagnostic_aggregator/GenericAnalyzer + path: emergency_stop + contains: [": aeb_emergency_stop"] + timeout: 1.0 + control_command_gate: type: diagnostic_aggregator/AnalyzerGroup path: control_command_gate diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 71dc2ac600685..312a2e6186c42 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -20,6 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -43,6 +44,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 9708456df4fe7..88431dc406fe3 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -20,6 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -43,6 +44,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default