From cc50b5808e23d2ea5773cb5961460f3639cbaaba Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 6 Aug 2024 15:19:31 +0900 Subject: [PATCH] delete old AEB package Signed-off-by: Daniel Sanchez --- .../CMakeLists.txt | 26 - .../autonomous_emergency_braking.param.yaml | 17 - .../autonomous_emergency_braking/node.hpp | 192 ------- .../autonomous_emergency_braking.launch.xml | 19 - .../autonomous_emergency_braking/package.xml | 41 -- .../autonomous_emergency_braking/src/node.cpp | 532 ------------------ 6 files changed, 827 deletions(-) delete mode 100644 control/autonomous_emergency_braking/CMakeLists.txt delete mode 100644 control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml delete mode 100644 control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp delete mode 100644 control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml delete mode 100644 control/autonomous_emergency_braking/package.xml delete mode 100644 control/autonomous_emergency_braking/src/node.cpp diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt deleted file mode 100644 index 6027aeac76333..0000000000000 --- a/control/autonomous_emergency_braking/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -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 deleted file mode 100644 index 09db0feb34597..0000000000000 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - 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 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp deleted file mode 100644 index 5865530ec1e3b..0000000000000 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ /dev/null @@ -1,192 +0,0 @@ -// 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 -#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; -using Vector3 = geometry_msgs::msg::Vector3; - -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 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::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(MarkerArray & debug_markers); - bool hasCollision( - const double current_v, const Path & ego_path, const std::vector & objects); - - 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, const rclcpp::Time & stamp, - std::vector & objects); - - void addMarker( - const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, - const std::vector & 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}; - Vector3::SharedPtr angular_velocity_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_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml deleted file mode 100644 index cf6640ec6be52..0000000000000 --- a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml deleted file mode 100644 index f68c16798ca79..0000000000000 --- a/control/autonomous_emergency_braking/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - 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 - 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 deleted file mode 100644 index 39419eeb6ae53..0000000000000 --- a/control/autonomous_emergency_braking/src/node.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// 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 - -#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()), - collision_data_keeper_(this->get_clock()) -{ - // 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"); - - const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); - collision_data_keeper_.setTimeout(collision_keeping_sec); - - // 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) -{ - // transform imu - 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; - } - - angular_velocity_ptr_ = std::make_shared(); - tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); -} - -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_ && !angular_velocity_ptr_) { - return missing("imu"); - } - - 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) -{ - MarkerArray debug_markers; - checkCollision(debug_markers); - - if (!collision_data_keeper_.checkExpired()) { - const std::string error_msg = "[AEB]: Emergency Brake"; - const auto diag_level = DiagnosticStatus::ERROR; - stat.summary(diag_level, error_msg); - const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - addCollisionMarker(data, debug_markers); - } else { - const std::string error_msg = "[AEB]: No Collision"; - const auto diag_level = DiagnosticStatus::OK; - stat.summary(diag_level, error_msg); - } - - // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); -} - -bool AEB::checkCollision(MarkerArray & debug_markers) -{ - // 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; - } - - // step3. create ego path based on sensor data - bool has_collision_ego = false; - if (use_imu_path_) { - Path ego_path; - std::vector ego_polys; - const double current_w = angular_velocity_ptr_->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); - - std::vector objects; - createObjectData(ego_path, ego_polys, current_time, objects); - has_collision_ego = hasCollision(current_v, ego_path, objects); - - std::string ns = "ego"; - addMarker( - current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns, - debug_markers); - } - - // step4. transform predicted trajectory from control module - bool has_collision_predicted = false; - if (use_predicted_trajectory_) { - Path predicted_path; - std::vector predicted_polys; - 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); - std::vector objects; - createObjectData(predicted_path, predicted_polys, current_time, objects); - has_collision_predicted = hasCollision(current_v, predicted_path, objects); - - std::string ns = "predicted"; - addMarker( - current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a, - ns, debug_markers); - } - - return has_collision_ego || has_collision_predicted; -} - -bool AEB::hasCollision( - const double current_v, const Path & ego_path, const std::vector & objects) -{ - // 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 = - tier4_autoware_utils::calcSignedArcLength(ego_path, current_p, obj.position) - - vehicle_info_.max_longitudinal_offset_m; - if (dist_ego_to_object < rss_dist) { - // collision happens - ObjectData collision_data = obj; - collision_data.rss = rss_dist; - collision_data.distance_to_object = dist_ego_to_object; - collision_data_keeper_.update(collision_data); - 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 (tier4_autoware_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, - const rclcpp::Time & stamp, std::vector & objects) -{ - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty()) { - return; - } - - 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.stamp = stamp; - 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 = tier4_autoware_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 std::vector & objects, const double color_r, const double color_g, - const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) -{ - auto path_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_path", 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) { - path_marker.points.at(i) = path.at(i).position; - } - debug_markers.markers.push_back(path_marker); - - auto polygon_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_polygon", 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); - polygon_marker.points.push_back(curr_point); - polygon_marker.points.push_back(next_point); - } - } - debug_markers.markers.push_back(polygon_marker); - - auto object_data_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - for (const auto & e : objects) { - object_data_marker.points.push_back(e.position); - } - debug_markers.markers.push_back(object_data_marker); -} - -void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) -{ - auto point_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); - point_marker.pose.position = data.position; - debug_markers.markers.push_back(point_marker); -} - -} // namespace autoware::motion::control::autonomous_emergency_braking - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB)