From 4508d23ffcb5df3c244f50b5f16ddf9f00536541 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 7 Feb 2024 12:58:40 +0900 Subject: [PATCH 1/3] fix(AEB): check empty path at low speed Signed-off-by: Mamoru Sobue --- .../autonomous_emergency_braking/node.hpp | 9 +-- .../autonomous_emergency_braking/src/node.cpp | 63 ++++++++++--------- 2 files changed, 40 insertions(+), 32 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 27ec775110109..14a532307180b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -140,10 +141,10 @@ class AEB : public rclcpp::Node 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); + std::optional generateEgoPath( + const double curr_v, const double curr_w, std::vector & polygons); + std::optional generateEgoPath( + const Trajectory & predicted_traj, std::vector & polygons); void createObjectData( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects); diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 960aadf226b70..52314b594c6db 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -317,22 +317,23 @@ bool AEB::checkCollision(MarkerArray & debug_markers) 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); + const auto ego_path_opt = generateEgoPath(current_v, current_w, ego_polys); + if (ego_path_opt) { + const auto & ego_path = ego_path_opt.value(); + 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; @@ -340,15 +341,18 @@ 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; - 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); + 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); + } } return has_collision_ego || has_collision_predicted; @@ -384,9 +388,10 @@ bool AEB::hasCollision( return false; } -void AEB::generateEgoPath( - const double curr_v, const double curr_w, Path & path, std::vector & polygons) +std::optional AEB::generateEgoPath( + const double curr_v, const double curr_w, std::vector & polygons) { + Path path; double curr_x = 0.0; double curr_y = 0.0; double curr_yaw = 0.0; @@ -397,7 +402,7 @@ void AEB::generateEgoPath( if (curr_v < 0.1) { // if current velocity is too small, assume it stops at the same point - return; + return std::nullopt; } constexpr double epsilon = 1e-6; @@ -435,16 +440,17 @@ void AEB::generateEgoPath( 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; } -void AEB::generateEgoPath( - const Trajectory & predicted_traj, Path & path, - std::vector & polygons) +std::optional AEB::generateEgoPath( + const Trajectory & predicted_traj, std::vector & polygons) { if (predicted_traj.points.empty()) { - return; + return std::nullopt; } + Path path; geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( @@ -452,7 +458,7 @@ void AEB::generateEgoPath( 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; + return std::nullopt; } // create path @@ -471,6 +477,7 @@ void AEB::generateEgoPath( 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; } void AEB::createObjectData( From f2f41058ebd4ad205a0782cca6b3586c18b5ac31 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 8 Feb 2024 18:13:33 +0900 Subject: [PATCH 2/3] fix Signed-off-by: Mamoru Sobue --- .../autonomous_emergency_braking/node.hpp | 3 +- .../autonomous_emergency_braking/src/node.cpp | 28 ++++++++----------- 2 files changed, 13 insertions(+), 18 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 14a532307180b..fb1973d777138 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -141,8 +141,7 @@ class AEB : public rclcpp::Node bool hasCollision( const double current_v, const Path & ego_path, const std::vector & objects); - std::optional generateEgoPath( - const double curr_v, const double curr_w, std::vector & polygons); + 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( diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 52314b594c6db..2b5553971a8d5 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -309,7 +309,6 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // 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; @@ -317,18 +316,15 @@ bool AEB::checkCollision(MarkerArray & debug_markers) 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_opt = generateEgoPath(current_v, current_w, ego_polys); - if (ego_path_opt) { - const auto & ego_path = ego_path_opt.value(); - 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); - } + 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); } // step4. transform predicted trajectory from control module @@ -388,7 +384,7 @@ bool AEB::hasCollision( return false; } -std::optional AEB::generateEgoPath( +Path AEB::generateEgoPath( const double curr_v, const double curr_w, std::vector & polygons) { Path path; @@ -402,7 +398,7 @@ std::optional AEB::generateEgoPath( if (curr_v < 0.1) { // if current velocity is too small, assume it stops at the same point - return std::nullopt; + return path; } constexpr double epsilon = 1e-6; @@ -450,7 +446,6 @@ std::optional AEB::generateEgoPath( return std::nullopt; } - Path path; geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( @@ -462,6 +457,7 @@ std::optional AEB::generateEgoPath( } // create path + Path path; path.resize(predicted_traj.points.size()); for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; From a8a704e04d2f453b09d8c15350557ee87c4be033 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 8 Feb 2024 18:18:30 +0900 Subject: [PATCH 3/3] add maintainer Signed-off-by: Mamoru Sobue --- control/autonomous_emergency_braking/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index d45078064da3c..1ac255c21921b 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -6,6 +6,7 @@ Autonomous Emergency Braking package as a ROS 2 node Takamasa Horibe Tomoya Kimura + Mamoru Sobue Apache License 2.0