From 474f82aa3bb70e1845c11c1eedb2c57372872cee Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 26 Apr 2024 11:18:15 +0900 Subject: [PATCH 1/5] feat(autonomous_emergency_braking): filter and crop aeb pointcloud (#6875) * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez * eliminate unused var Signed-off-by: Daniel Sanchez * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez * eliminate unused var Signed-off-by: Daniel Sanchez * add clustering method to eliminate small objects/noise Signed-off-by: Daniel Sanchez * remove comments Signed-off-by: Daniel Sanchez * Crop points outside of EGO predicted path Signed-off-by: Daniel Sanchez * remove offset Signed-off-by: Daniel Sanchez * Update library use Signed-off-by: Daniel Sanchez * add check for empty cloud Signed-off-by: Daniel Sanchez * add extra width for pointcloud cropping Signed-off-by: Daniel Sanchez * Use single PC ptr Signed-off-by: Daniel Sanchez * remove problematic option Signed-off-by: Daniel Sanchez * Revert "Use single PC ptr" This reverts commit b5091fc6c284cd3ee76b322db6f96b77efa0e37e. Signed-off-by: Daniel Sanchez * refactoring Signed-off-by: Daniel Sanchez * Add back timestamp Signed-off-by: Daniel Sanchez * consider all points in clusters Signed-off-by: Daniel Sanchez * USe object hull to detect collisions Signed-off-by: Daniel Sanchez * Use only closest object point Signed-off-by: Daniel Sanchez * remove unused functions Signed-off-by: Daniel Sanchez * remove debug timer out of code Signed-off-by: Daniel Sanchez * make it so the clustering uses parameters Signed-off-by: Daniel Sanchez * solve type problem Signed-off-by: Daniel Sanchez * update comment Signed-off-by: Daniel Sanchez * update README Signed-off-by: Daniel Sanchez * eliminate member var in favor of local pointcloud ptr Signed-off-by: Daniel Sanchez * remove unused chrono dependency Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/README.md | 14 +- .../autonomous_emergency_braking.param.yaml | 4 + .../autonomous_emergency_braking/node.hpp | 24 +- .../autonomous_emergency_braking/src/node.cpp | 294 ++++++++++++------ 4 files changed, 228 insertions(+), 108 deletions(-) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index 28b7636ba82ac..a4e737ca545be 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -86,20 +86,28 @@ where $v$ and $\omega$ are current longitudinal velocity and angular velocity re ### 3. Get target obstacles from the input point cloud -After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has two major steps, which are rough filtering and rigorous filtering. +After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. #### Rough filtering -In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default 5[m]) away from the predicted path of the ego vehicle and ignore the point cloud (obstacles) that are not within it. The image of the rough filtering is illustrated below. +In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The image of the rough filtering is illustrated below. ![rough_filtering](./image/obstacle_filtering_1.drawio.svg) +#### Noise filtering with clustering and convex hulls + +To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: . + +Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step. + #### Rigorous filtering -After rough filtering, it performs a geometric collision check to determine whether the filtered obstacles actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. +After Noise filtering, it performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept. ![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg) +Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe. + ### 4. Collision check with target obstacles In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as: diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 82d5a4f12e002..bb429662dabfb 100644 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,6 +3,7 @@ publish_debug_pointcloud: false use_predicted_trajectory: true use_imu_path: true + path_footprint_extra_margin: 1.0 detection_range_min_height: 0.0 detection_range_max_height_margin: 0.0 voxel_grid_x: 0.05 @@ -14,6 +15,9 @@ t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 + cluster_tolerance: 0.1 #[m] + minimum_cluster_size: 10 + maximum_cluster_size: 10000 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 1.5 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index b8aad1c80dfea..a30720e16a25e 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -64,7 +65,6 @@ 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; @@ -138,15 +138,19 @@ class AEB : public rclcpp::Node // 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); + bool hasCollision(const double current_v, const ObjectData & closest_object); + + Path generateEgoPath(const double curr_v, const double curr_w); + std::optional generateEgoPath(const Trajectory & predicted_traj); + std::vector generatePathFootprint(const Path & path, const double extra_width_margin); - Path generateEgoPath(const double curr_v, const double curr_w, std::vector & polygons); - std::optional generateEgoPath( - const Trajectory & predicted_traj, std::vector & polygons); - void createObjectData( + void createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); + std::vector & objects, + const pcl::PointCloud::Ptr obstacle_points_ptr); + + void cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, @@ -175,6 +179,7 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; @@ -186,6 +191,9 @@ class AEB : public rclcpp::Node double t_response_; double a_ego_min_; double a_obj_min_; + double cluster_tolerance_; + int minimum_cluster_size_; + int maximum_cluster_size_; double imu_prediction_time_horizon_; double imu_prediction_time_interval_; double mpc_prediction_time_horizon_; diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 8c830812078b6..87a1ffa19bc1d 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -15,14 +15,25 @@ #include "autonomous_emergency_braking/node.hpp" #include +#include +#include #include #include #include +#include +#include #include +#include +#include +#include #include #include +#include +#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -34,8 +45,6 @@ #include #endif -#include -#include namespace autoware::motion::control::autonomous_emergency_braking { using diagnostic_msgs::msg::DiagnosticStatus; @@ -129,6 +138,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = declare_parameter("detection_range_max_height_margin"); @@ -141,6 +151,11 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) t_response_ = declare_parameter("t_response"); a_ego_min_ = declare_parameter("a_ego_min"); a_obj_min_ = declare_parameter("a_obj_min"); + + cluster_tolerance_ = declare_parameter("cluster_tolerance"); + minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); + maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); + imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); @@ -152,7 +167,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) // 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)); + timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); } void AEB::onTimer() @@ -242,11 +257,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) 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; - if (publish_debug_pointcloud_) { - pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); - } } bool AEB::isDataReady() @@ -304,6 +317,8 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) bool AEB::checkCollision(MarkerArray & debug_markers) { + using colorTuple = std::tuple; + // step1. check data if (!isDataReady()) { return false; @@ -320,86 +335,101 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return false; } - // step3. create ego path based on sensor data - bool has_collision_ego = false; - if (use_imu_path_) { - std::vector ego_polys; + auto check_collision = [&]( + const auto & path, const colorTuple & debug_colors, + const std::string & debug_ns, const rclcpp::Time & current_time, + pcl::PointCloud::Ptr filtered_objects) { + // Crop out Pointcloud using an extra wide ego path + const auto expanded_ego_polys = + generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + + // Check which points of the cropped point cloud are on the ego path, and get the closest one + std::vector objects_from_point_clusters; + const auto ego_polys = generatePathFootprint(path, expand_width_); + createObjectDataUsingPointCloudClusters( + path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + + // Add debug markers + const auto [color_r, color_g, color_b, color_a] = debug_colors; + addMarker( + current_time, path, ego_polys, objects_from_point_clusters, color_r, color_g, color_b, + color_a, debug_ns, debug_markers); + + // Get only the closest object + const auto closest_object_point = std::min_element( + objects_from_point_clusters.begin(), objects_from_point_clusters.end(), + [](const auto & o1, const auto & o2) { + return o1.distance_to_object < o2.distance_to_object; + }); + + // check for empty vector + return (closest_object_point != objects_from_point_clusters.end()) + ? hasCollision(current_v, *closest_object_point) + : false; + }; + + // step3. make function to check collision with ego path created with sensor data + const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_imu_path_) return false; 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; + constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; + const std::string ns = "ego"; + const auto ego_path = generateEgoPath(current_v, current_w); const auto current_time = get_clock()->now(); - const auto ego_path = generateEgoPath(current_v, current_w, 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); - } + return check_collision(ego_path, debug_color, ns, current_time, filtered_objects); + }; - // step4. transform predicted trajectory from control module - bool has_collision_predicted = false; - if (use_predicted_trajectory_) { - std::vector predicted_polys; + // step4. make function to check collision with predicted trajectory from control module + const auto has_collision_predicted = + [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; 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 predicted_path_opt = generateEgoPath(*predicted_traj_ptr); + + if (!predicted_path_opt) return false; const auto current_time = predicted_traj_ptr->header.stamp; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys); - if (predicted_path_opt) { - const auto & predicted_path = predicted_path_opt.value(); - 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); - } - } + constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; + const std::string ns = "predicted"; + const auto & predicted_path = predicted_path_opt.value(); - return has_collision_ego || has_collision_predicted; + return check_collision(predicted_path, debug_color, ns, current_time, filtered_objects); + }; + + // Data of filtered point cloud + pcl::PointCloud::Ptr filtered_objects(new PointCloud); + // evaluate if there is a collision for both paths + const bool has_collision = + has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); + + // Debug print + if (!filtered_objects->empty() && publish_debug_pointcloud_) { + const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); + pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); + } + return has_collision; } -bool AEB::hasCollision( - const double current_v, const Path & ego_path, const std::vector & objects) +bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) { - // calculate RSS - const auto current_p = tier4_autoware_utils::createPoint( - ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); + const double & obj_v = closest_object.velocity; 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_; - - // check the object is front the ego or not - if ((obj.position.x - ego_path[0].position.x) > 0) { - 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 - 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; - } - } + 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_; + if (closest_object.distance_to_object < rss_dist) { + // collision happens + ObjectData collision_data = closest_object; + collision_data.rss = rss_dist; + collision_data.distance_to_object = closest_object.distance_to_object; + collision_data_keeper_.update(collision_data); + return true; } return false; } -Path AEB::generateEgoPath( - const double curr_v, const double curr_w, std::vector & polygons) +Path AEB::generateEgoPath(const double curr_v, const double curr_w) { Path path; double curr_x = 0.0; @@ -444,17 +474,10 @@ Path AEB::generateEgoPath( } 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_); - } return path; } -std::optional AEB::generateEgoPath( - const Trajectory & predicted_traj, std::vector & polygons) +std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) { if (predicted_traj.points.empty()) { return std::nullopt; @@ -482,35 +505,82 @@ std::optional AEB::generateEgoPath( break; } } - // create polygon - polygons.resize(path.size()); + return path; +} + +std::vector AEB::generatePathFootprint( + const Path & path, const double extra_width_margin) +{ + std::vector polygons; 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_); + polygons.push_back( + createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); } - return path; + return polygons; } -void AEB::createObjectData( - const Path & ego_path, const std::vector & ego_polys, - const rclcpp::Time & stamp, std::vector & objects) +void AEB::createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) { // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty()) { + if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->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) { + // eliminate noisy points by only considering points belonging to clusters of at least a certain + // size + const std::vector cluster_indices = std::invoke([&]() { + std::vector cluster_idx; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(obstacle_points_ptr); + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(cluster_tolerance_); + ec.setMinClusterSize(minimum_cluster_size_); + ec.setMaxClusterSize(maximum_cluster_size_); + ec.setSearchMethod(tree); + ec.setInputCloud(obstacle_points_ptr); + ec.extract(cluster_idx); + return cluster_idx; + }); + + PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); + for (const auto & indices : cluster_indices) { + PointCloud::Ptr cluster(new PointCloud); + for (const auto & index : indices.indices) { + cluster->push_back((*obstacle_points_ptr)[index]); + } + // Make a 2d convex hull for the objects + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(cluster); + std::vector polygons; + PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + for (const auto & p : *surface_hull) { + points_belonging_to_cluster_hulls->push_back(p); + } + } + + // select points inside the ego footprint path + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); + + for (const auto & p : *points_belonging_to_cluster_hulls) { + const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) - + vehicle_info_.max_longitudinal_offset_m; + // objects behind ego are ignored + if (dist_ego_to_object < 0.0) continue; + ObjectData obj; obj.stamp = stamp; - obj.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + obj.position = obj_position; 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; - } + obj.distance_to_object = dist_ego_to_object; + + const Point2d obj_point(p.x, p.y); for (const auto & ego_poly : ego_polys) { if (bg::within(obj_point, ego_poly)) { objects.push_back(obj); @@ -520,6 +590,36 @@ void AEB::createObjectData( } } +void AEB::cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) +{ + PointCloud::Ptr full_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); + // Create a Point cloud with the points of the ego footprint + PointCloud::Ptr path_polygon_points(new PointCloud); + std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) { + std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) { + pcl::PointXYZ point(p.x(), p.y(), 0.0); + path_polygon_points->push_back(point); + }); + }); + // Make a surface hull with the ego footprint to filter out points + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(path_polygon_points); + std::vector polygons; + pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + // Filter out points outside of the path's convex hull + pcl::CropHull path_polygon_hull_filter; + path_polygon_hull_filter.setDim(2); + path_polygon_hull_filter.setInputCloud(full_points_ptr); + path_polygon_hull_filter.setHullIndices(polygons); + path_polygon_hull_filter.setHullCloud(surface_hull); + path_polygon_hull_filter.filter(*filtered_objects); + filtered_objects->header.stamp = full_points_ptr->header.stamp; +} + 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, @@ -553,7 +653,7 @@ void AEB::addMarker( 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::createMarkerScale(0.5, 0.5, 0.5), 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); From ac8fff1bc4ee9aae22d435fdb96844cc544093ac Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 7 May 2024 19:47:43 +0900 Subject: [PATCH 2/5] feat(autonomous_emergency_braking): add obstacle velocity estimation for AEB (#6912) * enable aeb, fix topic problem Signed-off-by: Daniel Sanchez * add clustering method to eliminate small objects/noise Signed-off-by: Daniel Sanchez * remove comments Signed-off-by: Daniel Sanchez * Crop points outside of EGO predicted path Signed-off-by: Daniel Sanchez * remove offset Signed-off-by: Daniel Sanchez * Update library use Signed-off-by: Daniel Sanchez * add check for empty cloud Signed-off-by: Daniel Sanchez * add extra width for pointcloud cropping Signed-off-by: Daniel Sanchez * Use single PC ptr Signed-off-by: Daniel Sanchez * Revert "Use single PC ptr" This reverts commit b5091fc6c284cd3ee76b322db6f96b77efa0e37e. Signed-off-by: Daniel Sanchez * Add back timestamp Signed-off-by: Daniel Sanchez * USe object hull to detect collisions Signed-off-by: Daniel Sanchez * remove unused functions Signed-off-by: Daniel Sanchez * remove debug timer out of code Signed-off-by: Daniel Sanchez * eliminate member var in favor of local pointcloud ptr Signed-off-by: Daniel Sanchez * remove unused chrono dependency Signed-off-by: Daniel Sanchez * Add object history to etimate object speed with pointcloud Signed-off-by: Daniel Sanchez * Add median velocity calc Signed-off-by: Daniel Sanchez * add velocity thresholds Signed-off-by: Daniel Sanchez * refactoring Signed-off-by: Daniel Sanchez * cleaning Signed-off-by: Daniel Sanchez * refactor Signed-off-by: Daniel Sanchez * change voxwl Signed-off-by: Daniel Sanchez * readme Signed-off-by: Daniel Sanchez * remove mutex since they are not necessary Signed-off-by: Daniel Sanchez * add markers for closest object velocity Signed-off-by: Daniel Sanchez * added units to marker Signed-off-by: Daniel Sanchez * delete duplicated param Signed-off-by: Daniel Sanchez * delete duplicated param Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/README.md | 27 +++ .../autonomous_emergency_braking.param.yaml | 36 ++-- .../autonomous_emergency_braking/node.hpp | 158 ++++++++++++++++-- .../autonomous_emergency_braking/src/node.cpp | 133 +++++++++------ 4 files changed, 277 insertions(+), 77 deletions(-) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index a4e737ca545be..7a7bf212320e0 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -108,6 +108,33 @@ After Noise filtering, it performs a geometric collision check to determine whet Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe. +#### Obstacle velocity estimation + +Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations: + +$$ +d_{t} = o_{time stamp} - prev_{time stamp} +$$ + +$$ +d_{pos} = norm(o_{pos} - prev_{pos}) +$$ + +$$ +v_{norm} = d_{pos} / d_{t} +$$ + +Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively. + +Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: + +$$ +v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego} +$$ + +where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object. +All these equations are performed disregarding the z axis (in 2D). + ### 4. Collision check with target obstacles In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as: diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index bb429662dabfb..68a42383feb1f 100644 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -1,26 +1,38 @@ /**: ros__parameters: - publish_debug_pointcloud: false + # Ego path calculation use_predicted_trajectory: true - use_imu_path: true - path_footprint_extra_margin: 1.0 + use_imu_path: false + min_generated_path_length: 0.5 + imu_prediction_time_horizon: 1.5 + imu_prediction_time_interval: 0.1 + mpc_prediction_time_horizon: 1.5 + mpc_prediction_time_interval: 0.1 + + # Debug + publish_debug_pointcloud: false + + # Point cloud partitioning detection_range_min_height: 0.0 detection_range_max_height_margin: 0.0 voxel_grid_x: 0.05 voxel_grid_y: 0.05 voxel_grid_z: 100000.0 - min_generated_path_length: 0.5 + + # Point cloud cropping expand_width: 0.1 + path_footprint_extra_margin: 4.0 + + # Point cloud clustering + cluster_tolerance: 0.1 #[m] + minimum_cluster_size: 10 + maximum_cluster_size: 10000 + + # RSS distance collision check longitudinal_offset: 2.0 t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 - cluster_tolerance: 0.1 #[m] - minimum_cluster_size: 10 - maximum_cluster_size: 10000 - imu_prediction_time_horizon: 1.5 - imu_prediction_time_interval: 0.1 - mpc_prediction_time_horizon: 1.5 - mpc_prediction_time_interval: 0.1 - collision_keeping_sec: 0.0 + collision_keeping_sec: 2.0 + previous_obstacle_keep_time: 1.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 index a30720e16a25e..e2e2d0ae6745a 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -16,6 +16,7 @@ #define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #include +#include #include #include #include @@ -40,12 +41,13 @@ #include #include +#include +#include #include -#include #include #include +#include #include - namespace autoware::motion::control::autonomous_emergency_braking { @@ -79,30 +81,142 @@ class CollisionDataKeeper public: explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } - void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; } + void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) + { + collision_keep_time_ = collision_keep_time; + previous_obstacle_keep_time_ = previous_obstacle_keep_time; + } - bool checkExpired() + bool checkObjectDataExpired(std::optional & data, const double timeout) { - if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) { - data_.reset(); + if (!data.has_value()) return true; + const auto now = clock_->now(); + const auto & prev_obj = data.value(); + const auto & data_time_stamp = prev_obj.stamp; + if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { + data = std::nullopt; + return true; } - return (data_ == nullptr); + return false; + } + + bool checkCollisionExpired() + { + return this->checkObjectDataExpired(closest_object_, collision_keep_time_); } - void update(const ObjectData & data) { data_.reset(new ObjectData(data)); } + bool checkPreviousObjectDataExpired() + { + return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); + } - ObjectData get() + ObjectData get() const { - if (data_) { - return *data_; - } else { - return ObjectData(); + return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); + } + + ObjectData getPreviousObjectData() const + { + return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); + } + + void setCollisionData(const ObjectData & data) + { + closest_object_ = std::make_optional(data); + } + + void setPreviousObjectData(const ObjectData & data) + { + prev_closest_object_ = std::make_optional(data); + } + + void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + + void updateVelocityHistory( + const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) + { + // remove old msg from deque + const auto now = clock_->now(); + std::remove_if( + obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), + [&](const auto & velocity_time_pair) { + const auto & vel_time = velocity_time_pair.second; + return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); + }); + + obstacle_velocity_history_.emplace_back( + std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + } + + std::optional getMedianObstacleVelocity() const + { + if (obstacle_velocity_history_.empty()) return std::nullopt; + std::vector raw_velocities; + for (const auto & vel_time_pair : obstacle_velocity_history_) { + raw_velocities.emplace_back(vel_time_pair.first); + } + + const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 + : (raw_velocities.size()) / 2.0; + const size_t med2 = (raw_velocities.size()) / 2.0; + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); + const double vel1 = raw_velocities.at(med1); + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); + const double vel2 = raw_velocities.at(med2); + return (vel1 + vel2) / 2.0; + } + + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed) + { + if (this->checkPreviousObjectDataExpired()) { + this->setPreviousObjectData(closest_object); + this->resetVelocityHistory(); + return std::nullopt; + } + + const auto estimated_velocity_opt = std::invoke([&]() -> std::optional { + const auto & prev_object = this->getPreviousObjectData(); + const double p_dt = + (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; + if (p_dt < std::numeric_limits::epsilon()) return std::nullopt; + const auto & nearest_collision_point = closest_object.position; + const auto & prev_collision_point = prev_object.position; + + const double p_dx = nearest_collision_point.x - prev_collision_point.x; + const double p_dy = nearest_collision_point.y - prev_collision_point.y; + const double p_dist = std::hypot(p_dx, p_dy); + const double p_yaw = std::atan2(p_dy, p_dx); + const double p_vel = p_dist / p_dt; + + const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); + const auto & nearest_path_pose = path.at(nearest_idx); + const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); + const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; + + // Current RSS distance calculation does not account for negative velocities + return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; + }); + + if (!estimated_velocity_opt.has_value()) { + this->setPreviousObjectData(closest_object); + this->resetVelocityHistory(); + return std::nullopt; } + + const auto & estimated_velocity = estimated_velocity_opt.value(); + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(estimated_velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); } private: - std::unique_ptr data_; - double timeout_sec_{0.0}; + std::optional prev_closest_object_{std::nullopt}; + std::optional closest_object_{std::nullopt}; + double collision_keep_time_{0.0}; + double previous_obstacle_keep_time_{0.0}; + + std::deque> obstacle_velocity_history_; rclcpp::Clock::SharedPtr clock_; }; @@ -152,14 +266,22 @@ class AEB : public rclcpp::Node void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); + void createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); + 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); + const std::vector & objects, const std::optional & closest_object, + 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); + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed); + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 87a1ffa19bc1d..9847d7aae63b5 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -14,7 +14,6 @@ #include "autonomous_emergency_braking/node.hpp" -#include #include #include #include @@ -107,33 +106,37 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) 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_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_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_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_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)); + 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); - + { + 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); - + { + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); @@ -161,8 +164,12 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); - const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); - collision_data_keeper_.setTimeout(collision_keeping_sec); + { // Object history data keeper setup + const auto previous_obstacle_keep_time = + declare_parameter("previous_obstacle_keep_time"); + const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } // start time const double aeb_hz = declare_parameter("aeb_hz"); @@ -297,13 +304,14 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) MarkerArray debug_markers; checkCollision(debug_markers); - if (!collision_data_keeper_.checkExpired()) { + if (!collision_data_keeper_.checkCollisionExpired()) { 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); + stat.addf("Object Speed", "%.2f", data.velocity); addCollisionMarker(data, debug_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -337,7 +345,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) auto check_collision = [&]( const auto & path, const colorTuple & debug_colors, - const std::string & debug_ns, const rclcpp::Time & current_time, + const std::string & debug_ns, pcl::PointCloud::Ptr filtered_objects) { // Crop out Pointcloud using an extra wide ego path const auto expanded_ego_polys = @@ -347,25 +355,40 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Check which points of the cropped point cloud are on the ego path, and get the closest one std::vector objects_from_point_clusters; const auto ego_polys = generatePathFootprint(path, expand_width_); + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; createObjectDataUsingPointCloudClusters( path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + // Get only the closest object and calculate its speed + const auto closest_object_point = std::invoke([&]() -> std::optional { + const auto closest_object_point_itr = std::min_element( + objects_from_point_clusters.begin(), objects_from_point_clusters.end(), + [](const auto & o1, const auto & o2) { + return o1.distance_to_object < o2.distance_to_object; + }); + + if (closest_object_point_itr == objects_from_point_clusters.end()) { + return std::nullopt; + } + const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_point_itr, path, current_v); + if (!closest_object_speed.has_value()) { + return std::nullopt; + } + closest_object_point_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_point_itr); + }); + // Add debug markers - const auto [color_r, color_g, color_b, color_a] = debug_colors; - addMarker( - current_time, path, ego_polys, objects_from_point_clusters, color_r, color_g, color_b, - color_a, debug_ns, debug_markers); - - // Get only the closest object - const auto closest_object_point = std::min_element( - objects_from_point_clusters.begin(), objects_from_point_clusters.end(), - [](const auto & o1, const auto & o2) { - return o1.distance_to_object < o2.distance_to_object; - }); - - // check for empty vector - return (closest_object_point != objects_from_point_clusters.end()) - ? hasCollision(current_v, *closest_object_point) + { + const auto [color_r, color_g, color_b, color_a] = debug_colors; + addMarker( + this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, + closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); + } + // check collision using rss distance + return (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) : false; }; @@ -376,9 +399,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; const std::string ns = "ego"; const auto ego_path = generateEgoPath(current_v, current_w); - const auto current_time = get_clock()->now(); - return check_collision(ego_path, debug_color, ns, current_time, filtered_objects); + return check_collision(ego_path, debug_color, ns, filtered_objects); }; // step4. make function to check collision with predicted trajectory from control module @@ -389,12 +411,11 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); if (!predicted_path_opt) return false; - const auto current_time = predicted_traj_ptr->header.stamp; constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; const std::string ns = "predicted"; const auto & predicted_path = predicted_path_opt.value(); - return check_collision(predicted_path, debug_color, ns, current_time, filtered_objects); + return check_collision(predicted_path, debug_color, ns, filtered_objects); }; // Data of filtered point cloud @@ -423,7 +444,7 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object ObjectData collision_data = closest_object; collision_data.rss = rss_dist; collision_data.distance_to_object = closest_object.distance_to_object; - collision_data_keeper_.update(collision_data); + collision_data_keeper_.setCollisionData(collision_data); return true; } return false; @@ -622,8 +643,9 @@ void AEB::cropPointCloudWithEgoFootprintPath( 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) + const std::vector & objects, const std::optional & closest_object, + 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, @@ -659,6 +681,23 @@ void AEB::addMarker( object_data_marker.points.push_back(e.position); } debug_markers.markers.push_back(object_data_marker); + + // Visualize planner type text + if (closest_object.has_value()) { + const auto & obj = closest_object.value(); + const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker( + "base_link", obj.stamp, ns + "_closest_object_velocity", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color); + closest_object_velocity_marker_array.pose.position = obj.position; + const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; + closest_object_velocity_marker_array.text = + "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]"; + debug_markers.markers.push_back(closest_object_velocity_marker_array); + } } void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) From d55244c15d13353b5ae282dd2fbd62a37648fd73 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 10 May 2024 13:51:09 +0900 Subject: [PATCH 3/5] feat(autonomous_emergency_braking): add param update support for AEB (#6956) add param update support for AEB Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 9 ++++ .../autonomous_emergency_braking/src/node.cpp | 49 ++++++++++++++++++- 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index e2e2d0ae6745a..b5ddb7bf309d7 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -87,6 +87,11 @@ class CollisionDataKeeper previous_obstacle_keep_time_ = previous_obstacle_keep_time; } + std::pair getTimeout() + { + return {collision_keep_time_, previous_obstacle_keep_time_}; + } + bool checkObjectDataExpired(std::optional & data, const double timeout) { if (!data.has_value()) return true; @@ -246,6 +251,8 @@ class AEB : public rclcpp::Node void onTimer(); void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); void onAutowareState(const AutowareState::ConstSharedPtr input_msg); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); bool isDataReady(); @@ -321,6 +328,8 @@ class AEB : public rclcpp::Node double mpc_prediction_time_horizon_; double mpc_prediction_time_interval_; CollisionDataKeeper collision_data_keeper_; + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; }; } // namespace autoware::motion::control::autonomous_emergency_braking diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 9847d7aae63b5..d8886672a8ecd 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -34,7 +35,6 @@ #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #include @@ -171,12 +171,59 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); } + // Parameter Callback + set_param_res_ = + add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); + // start time const double aeb_hz = declare_parameter("aeb_hz"); const auto period_ns = rclcpp::Rate(aeb_hz).period(); timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); } +rcl_interfaces::msg::SetParametersResult AEB::onParameter( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); + updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam(parameters, "detection_range_min_height", detection_range_min_height_); + updateParam( + parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); + updateParam(parameters, "voxel_grid_x", voxel_grid_x_); + updateParam(parameters, "voxel_grid_y", voxel_grid_y_); + updateParam(parameters, "voxel_grid_z", voxel_grid_z_); + updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "expand_width", expand_width_); + updateParam(parameters, "longitudinal_offset", longitudinal_offset_); + updateParam(parameters, "t_response", t_response_); + updateParam(parameters, "a_ego_min", a_ego_min_); + updateParam(parameters, "a_obj_min", a_obj_min_); + + updateParam(parameters, "cluster_tolerance", cluster_tolerance_); + updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); + updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); + + updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); + updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); + updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); + updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); + + { // Object history data keeper setup + auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); + updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); + updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void AEB::onTimer() { updater_.force_update(); From cff39b21397eb4d7fcd6f10ecee643209e48b9d8 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 10 May 2024 14:10:08 +0900 Subject: [PATCH 4/5] fix(autonomous_emergency_braking): add missing erase to velocity history (#6966) add missing erase Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index b5ddb7bf309d7..43fb310b17416 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -142,13 +142,14 @@ class CollisionDataKeeper { // remove old msg from deque const auto now = clock_->now(); - std::remove_if( - obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), - [&](const auto & velocity_time_pair) { - const auto & vel_time = velocity_time_pair.second; - return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); - }); - + obstacle_velocity_history_.erase( + std::remove_if( + obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), + [&](const auto & velocity_time_pair) { + const auto & vel_time = velocity_time_pair.second; + return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); + }), + obstacle_velocity_history_.end()); obstacle_velocity_history_.emplace_back( std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); } @@ -204,8 +205,6 @@ class CollisionDataKeeper }); if (!estimated_velocity_opt.has_value()) { - this->setPreviousObjectData(closest_object); - this->resetVelocityHistory(); return std::nullopt; } From c1a4c43ecdeef8967a08ace83e6c264555adcc40 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 10 May 2024 17:02:24 +0900 Subject: [PATCH 5/5] feat(control_evaluator): implement a control evaluator (#6959) * add control evaluator module Signed-off-by: Daniel Sanchez * make the evaluator depend on messages from AEB Signed-off-by: Daniel Sanchez * update output msg Signed-off-by: Daniel Sanchez * delete extra new line Signed-off-by: Daniel Sanchez * update/fix details Signed-off-by: Daniel Sanchez * add a package mantainer Signed-off-by: Daniel Sanchez * Add a timer to maintain a constant rate of msg publishing Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- evaluator/control_evaluator/CMakeLists.txt | 23 +++++ evaluator/control_evaluator/README.md | 5 ++ .../control_evaluator_node.hpp | 61 +++++++++++++ .../launch/control_evaluator.launch.xml | 12 +++ evaluator/control_evaluator/package.xml | 29 +++++++ .../param/control_evaluator.defaults.yaml | 2 + .../src/control_evaluator_node.cpp | 86 +++++++++++++++++++ .../launch/control.launch.py | 17 ++++ launch/tier4_control_launch/package.xml | 1 + 9 files changed, 236 insertions(+) create mode 100644 evaluator/control_evaluator/CMakeLists.txt create mode 100644 evaluator/control_evaluator/README.md create mode 100644 evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp create mode 100644 evaluator/control_evaluator/launch/control_evaluator.launch.xml create mode 100644 evaluator/control_evaluator/package.xml create mode 100644 evaluator/control_evaluator/param/control_evaluator.defaults.yaml create mode 100644 evaluator/control_evaluator/src/control_evaluator_node.cpp diff --git a/evaluator/control_evaluator/CMakeLists.txt b/evaluator/control_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..b97a14e29cdcd --- /dev/null +++ b/evaluator/control_evaluator/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(control_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/${PROJECT_NAME}_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "control_diagnostics::controlEvaluatorNode" + EXECUTABLE ${PROJECT_NAME} +) + + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/control_evaluator/README.md b/evaluator/control_evaluator/README.md new file mode 100644 index 0000000000000..71bd5c0142570 --- /dev/null +++ b/evaluator/control_evaluator/README.md @@ -0,0 +1,5 @@ +# Planning Evaluator + +## Purpose + +This package provides nodes that generate metrics to evaluate the quality of control. diff --git a/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp b/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp new file mode 100644 index 0000000000000..3a3ba47d88e03 --- /dev/null +++ b/evaluator/control_evaluator/include/control_evaluator/control_evaluator_node.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 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 CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ +#define CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +#include +#include +#include +#include +#include + +namespace control_diagnostics +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; + +/** + * @brief Node for control evaluation + */ +class controlEvaluatorNode : public rclcpp::Node +{ +public: + explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options); + + /** + * @brief publish the given metric statistic + */ + DiagnosticStatus generateDiagnosticStatus(const bool is_emergency_brake) const; + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + void onTimer(); + +private: + rclcpp::Subscription::SharedPtr control_diag_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + + // Calculator + // Metrics + std::deque stamps_; + DiagnosticArray metrics_msg_; + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace control_diagnostics + +#endif // CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ diff --git a/evaluator/control_evaluator/launch/control_evaluator.launch.xml b/evaluator/control_evaluator/launch/control_evaluator.launch.xml new file mode 100644 index 0000000000000..75012791888a6 --- /dev/null +++ b/evaluator/control_evaluator/launch/control_evaluator.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/evaluator/control_evaluator/package.xml b/evaluator/control_evaluator/package.xml new file mode 100644 index 0000000000000..10686e9b73761 --- /dev/null +++ b/evaluator/control_evaluator/package.xml @@ -0,0 +1,29 @@ + + + + control_evaluator + 0.1.0 + ROS 2 node for evaluating control + Daniel SANCHEZ + takayuki MUROOKA + Apache License 2.0 + + Daniel SANCHEZ + takayuki MUROOKA + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + pluginlib + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/control_evaluator/param/control_evaluator.defaults.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/evaluator/control_evaluator/param/control_evaluator.defaults.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/evaluator/control_evaluator/src/control_evaluator_node.cpp b/evaluator/control_evaluator/src/control_evaluator_node.cpp new file mode 100644 index 0000000000000..d575a35a2389f --- /dev/null +++ b/evaluator/control_evaluator/src/control_evaluator_node.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 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 "control_evaluator/control_evaluator_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace control_diagnostics +{ +controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("control_evaluator", node_options) +{ + using std::placeholders::_1; + + control_diag_sub_ = create_subscription( + "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1)); + + // Publisher + metrics_pub_ = create_publisher("~/metrics", 1); + + // Timer callback to publish evaluator diagnostics + using namespace std::literals::chrono_literals; + timer_ = + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); +} + +DiagnosticStatus controlEvaluatorNode::generateDiagnosticStatus(const bool is_emergency_brake) const +{ + DiagnosticStatus status; + status.level = status.OK; + status.name = "autonomous_emergency_braking"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "decision"; + key_value.value = (is_emergency_brake) ? "stop" : "none"; + status.values.push_back(key_value); + return status; +} + +void controlEvaluatorNode::onTimer() +{ + if (!metrics_msg_.status.empty()) { + metrics_pub_->publish(metrics_msg_); + metrics_msg_.status.clear(); + } +} + +void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +{ + const auto start = now(); + const auto aeb_status = + std::find_if(diag_msg->status.begin(), diag_msg->status.end(), [](const auto & status) { + const bool aeb_found = status.name.find("autonomous_emergency_braking") != std::string::npos; + return aeb_found; + }); + + if (aeb_status == diag_msg->status.end()) return; + + const bool is_emergency_brake = (aeb_status->level == DiagnosticStatus::ERROR); + metrics_msg_.header.stamp = now(); + metrics_msg_.status.emplace_back(generateDiagnosticStatus(is_emergency_brake)); + + const auto runtime = (now() - start).seconds(); + RCLCPP_DEBUG(get_logger(), "control evaluation calculation time: %2.2f ms", runtime * 1e3); +} + +} // namespace control_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index a63e7f547fef1..2ef2e07367f81 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -343,6 +343,22 @@ def launch_setup(context, *args, **kwargs): ], ) + # control evaluator + control_evaluator_component = ComposableNode( + package="control_evaluator", + plugin="control_diagnostics::controlEvaluatorNode", + name="control_evaluator", + remappings=[ + ("~/input/diagnostics", "/diagnostics"), + ("~/output/metrics", "~/metrics"), + ], + ) + + control_evaluator_loader = LoadComposableNodes( + composable_node_descriptions=[control_evaluator_component], + target_container="/control/control_container", + ) + # control validator checker control_validator_component = ComposableNode( package="control_validator", @@ -369,6 +385,7 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_loader, autonomous_emergency_braking_loader, predicted_path_checker_loader, + control_evaluator_loader, ] ) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 4bfefa4d93747..801fa274dd086 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + control_evaluator external_cmd_converter external_cmd_selector lane_departure_checker