diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 214a6dc309210..32bedd15c5c53 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -341,7 +341,7 @@ class AEB : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; - rclcpp::Publisher::SharedPtr info_marker_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b3988ba238fe7..5d12031f9039b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -138,7 +139,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); - info_marker_publisher_ = this->create_publisher("~/info/markers", 1); + virtual_wall_publisher_ = this->create_publisher("~/virtual_wall", 1); debug_rss_distance_publisher_ = this->create_publisher("~/debug/rss_distance", 1); } @@ -398,7 +399,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; - MarkerArray info_markers; + MarkerArray virtual_wall_marker; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -414,7 +415,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) addCollisionMarker(data.value(), debug_markers); } } - addVirtualStopWallMarker(info_markers); + addVirtualStopWallMarker(virtual_wall_marker); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -423,7 +424,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) // publish debug markers debug_marker_publisher_->publish(debug_markers); - info_marker_publisher_->publish(info_markers); + virtual_wall_publisher_->publish(virtual_wall_marker); } bool AEB::checkCollision(MarkerArray & debug_markers) @@ -642,7 +643,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); const double & dt = imu_prediction_time_interval_; - const double distance_between_points = curr_v * dt; + const double distance_between_points = std::abs(curr_v) * dt; constexpr double minimum_distance_between_points{1e-2}; // if current velocity is too small, assume it stops at the same point // if distance between points is too small, arc length calculation is unreliable, so we skip @@ -887,7 +888,11 @@ void AEB::getClosestObjectsOnPath( if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } - + const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path); + if (!ego_is_driving_forward_opt.has_value()) { + return; + } + const bool ego_is_driving_forward = ego_is_driving_forward_opt.value(); // select points inside the ego footprint path const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); @@ -916,11 +921,9 @@ void AEB::getClosestObjectsOnPath( // If the object is behind the ego, we need to use the backward long offset. The distance should // be a positive number in any case - const bool is_object_in_front_of_ego = obj_arc_length > 0.0; - const double dist_ego_to_object = (is_object_in_front_of_ego) + const double dist_ego_to_object = (ego_is_driving_forward) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; - ObjectData obj; obj.stamp = stamp; obj.position = obj_position;