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..6d642ff5eb2e7 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,8 @@ publish_debug_pointcloud: false use_predicted_trajectory: true use_imu_path: true + crop_pointcloud_with_path_footprint: 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 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 b8168a9af063d..28879f17950a5 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -141,16 +141,18 @@ class AEB : public rclcpp::Node bool hasCollision( const double current_v, const Path & ego_path, const std::vector & objects); - Path generateEgoPath(const double curr_v, const double curr_w, std::vector & polygons); - std::optional generateEgoPath( - const Trajectory & predicted_traj, std::vector & polygons); + 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); + void createObjectData( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects); - void cropPointCloudWithEgoPath(const std::vector & ego_polys); + void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - void createClusteredPointCloudObjectData( + void createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects); @@ -183,6 +185,8 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + bool crop_pointcloud_with_path_footprint_; + double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 591bff6dd40b3..1fdc580941649 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -129,6 +129,9 @@ 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"); + crop_pointcloud_with_path_footprint_ = + declare_parameter("crop_pointcloud_with_path_footprint"); + 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"); @@ -322,23 +325,30 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // step3. create ego path based on sensor data bool has_collision_ego = false; if (use_imu_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(); - const auto ego_path = generateEgoPath(current_v, current_w, ego_polys); + const auto ego_path = generateEgoPath(current_v, current_w); - std::vector objects_cluster; - cropPointCloudWithEgoPath(ego_polys); - createClusteredPointCloudObjectData(ego_path, ego_polys, current_time, objects_cluster); - has_collision_ego = hasCollision(current_v, ego_path, objects_cluster); + // Crop out Pointcloud using an extra wide ego path + if (crop_pointcloud_with_path_footprint_) { + const auto expanded_ego_polys = + generatePathFootprint(ego_path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys); + } + + std::vector objects_from_point_clusters; + const auto ego_polys = generatePathFootprint(ego_path, expand_width_); + createObjectDataUsingPointCloudClusters( + ego_path, ego_polys, current_time, objects_from_point_clusters); + has_collision_ego = hasCollision(current_v, ego_path, objects_from_point_clusters); std::string ns = "ego"; addMarker( - current_time, ego_path, ego_polys, objects_cluster, color_r, color_g, color_b, color_a, ns, - debug_markers); + current_time, ego_path, ego_polys, objects_from_point_clusters, color_r, color_g, color_b, + color_a, ns, debug_markers); } // step4. transform predicted trajectory from control module @@ -351,19 +361,28 @@ bool AEB::checkCollision(MarkerArray & debug_markers) constexpr double color_b = 0.0; constexpr double color_a = 0.999; const auto current_time = predicted_traj_ptr->header.stamp; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys); + const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); if (predicted_path_opt) { const auto & predicted_path = predicted_path_opt.value(); - std::vector objects_cluster; - cropPointCloudWithEgoPath(predicted_polys); - createClusteredPointCloudObjectData( - predicted_path, predicted_polys, current_time, objects_cluster); - has_collision_predicted = hasCollision(current_v, predicted_path, objects_cluster); + // Crop out Pointcloud using an extra wide ego path + if (crop_pointcloud_with_path_footprint_) { + const auto expanded_ego_polys = + generatePathFootprint(predicted_path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys); + } + + std::vector objects_from_point_clusters; + const auto predicted_polys = generatePathFootprint(predicted_path, expand_width_); + cropPointCloudWithEgoFootprintPath(predicted_polys); + createObjectDataUsingPointCloudClusters( + predicted_path, predicted_polys, current_time, objects_from_point_clusters); + has_collision_predicted = + hasCollision(current_v, predicted_path, objects_from_point_clusters); std::string ns = "predicted"; addMarker( - current_time, predicted_path, predicted_polys, objects_cluster, color_r, color_g, color_b, - color_a, ns, debug_markers); + current_time, predicted_path, predicted_polys, objects_from_point_clusters, color_r, + color_g, color_b, color_a, ns, debug_markers); } } @@ -404,8 +423,7 @@ bool AEB::hasCollision( 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; @@ -450,17 +468,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; @@ -488,12 +499,18 @@ 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( @@ -526,7 +543,7 @@ void AEB::createObjectData( } } -void AEB::cropPointCloudWithEgoPath(const std::vector & ego_polys) +void AEB::cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys) { PointCloud::Ptr full_points_ptr(new PointCloud); pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); @@ -564,7 +581,7 @@ void AEB::cropPointCloudWithEgoPath(const std::vector & ego_polys) } } -void AEB::createClusteredPointCloudObjectData( +void AEB::createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects) {