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..fb1973d777138 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,9 @@ 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); + 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( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects); 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 diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 960aadf226b70..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,8 +316,7 @@ 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); - + 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); @@ -332,7 +330,6 @@ bool AEB::checkCollision(MarkerArray & 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 +337,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 +384,10 @@ bool AEB::hasCollision( return false; } -void AEB::generateEgoPath( - const double curr_v, const double curr_w, Path & path, std::vector & polygons) +Path 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 +398,7 @@ void AEB::generateEgoPath( if (curr_v < 0.1) { // if current velocity is too small, assume it stops at the same point - return; + return path; } constexpr double epsilon = 1e-6; @@ -435,14 +436,14 @@ 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; } geometry_msgs::msg::TransformStamped transform_stamped{}; @@ -452,10 +453,11 @@ 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 + 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; @@ -471,6 +473,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(