From 61d242de2670647f7e479923ed633ea13fb3e88d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 19 Jun 2024 17:04:27 +0900 Subject: [PATCH 01/28] feat(autonomous_emergency_braking): add predicted object support for aeb (#7548) * add polling sub to predicted objects Signed-off-by: Daniel Sanchez * WIP requires changing path frame to map Signed-off-by: Daniel Sanchez * add parameters and reuse predicted obj speed Signed-off-by: Daniel Sanchez * introduce early break to reduce computation time Signed-off-by: Daniel Sanchez * resolve merge conflicts Signed-off-by: Daniel Sanchez * fix guard Signed-off-by: Daniel Sanchez * remove unused declaration Signed-off-by: Daniel Sanchez * fix include Signed-off-by: Daniel Sanchez * fix include issues Signed-off-by: Daniel Sanchez * remove inline Signed-off-by: Daniel Sanchez * delete unused dependencies Signed-off-by: Daniel Sanchez * add utils.cpp Signed-off-by: Daniel Sanchez * remove _ for non member variable Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../CMakeLists.txt | 7 + .../autonomous_emergency_braking.param.yaml | 2 + .../autonomous_emergency_braking/node.hpp | 22 ++- .../autonomous_emergency_braking/utils.hpp | 88 +++++++++ ...re_autonomous_emergency_braking.launch.xml | 2 + .../src/node.cpp | 174 +++++++++++++++--- .../src/utils.cpp | 150 +++++++++++++++ .../launch/control.launch.py | 1 + 8 files changed, 418 insertions(+), 28 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp create mode 100644 control/autoware_autonomous_emergency_braking/src/utils.cpp diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 7f38e4387b452..85290c6c66730 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -12,11 +12,18 @@ include_directories( ${PCL_INCLUDE_DIRS} ) +ament_auto_add_library(autoware_autonomous_emergency_braking_helpers SHARED + include/autoware/autonomous_emergency_braking/utils.hpp + src/utils.cpp +) + set(AEB_NODE ${PROJECT_NAME}_node) ament_auto_add_library(${AEB_NODE} SHARED + include/autoware/autonomous_emergency_braking/node.hpp src/node.cpp ) +target_link_libraries(${AEB_NODE} autoware_autonomous_emergency_braking_helpers) rclcpp_components_register_node(${AEB_NODE} PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" EXECUTABLE ${PROJECT_NAME} diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 4233f8b6bebcb..344d076197170 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,6 +3,8 @@ # Ego path calculation use_predicted_trajectory: true use_imu_path: false + use_pointcloud_data: true + use_predicted_object_data: true use_object_velocity_calculation: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 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 7e06af73b0f11..1054ad8f57c1b 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 @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,9 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; + struct ObjectData { rclcpp::Time stamp; @@ -176,6 +180,13 @@ class CollisionDataKeeper std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { + // in case the object comes from predicted objects info, we reuse the speed. + if (closest_object.velocity > 0.0) { + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(closest_object.velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + if (this->checkPreviousObjectDataExpired()) { this->setPreviousObjectData(closest_object); this->resetVelocityHistory(); @@ -243,7 +254,9 @@ class AEB : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware_universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + this, "~/input/objects"}; + autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; @@ -275,6 +288,10 @@ class AEB : public rclcpp::Node std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + void createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); @@ -298,6 +315,7 @@ class AEB : public rclcpp::Node VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -313,6 +331,8 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + bool use_pointcloud_data_; + bool use_predicted_object_data_; bool use_object_velocity_calculation_; double path_footprint_extra_margin_; double detection_range_min_height_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp new file mode 100644 index 0000000000000..b046acd9153e9 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -0,0 +1,88 @@ +// 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 AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ +#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; + +/** + * @brief Apply a transform to a predicted object + * @param input the predicted object + * @param transform_stamped the tf2 transform + */ +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped); + +/** + * @brief Get the predicted objects polygon as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects cylindrical shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects bounding box shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param obj the object + */ +Polygon2d convertObjToPolygon(const PredictedObject & obj); +} // namespace autoware::motion::control::autonomous_emergency_braking::utils + +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index 75b1a9dcf822d..fe0df41ca89c7 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -5,6 +5,7 @@ + @@ -15,5 +16,6 @@ + diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 90cf3f900673e..a2d94c06e9fe1 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/autonomous_emergency_braking/node.hpp" - +#include +#include #include #include #include @@ -21,7 +21,11 @@ #include #include +#include + #include +#include +#include #include #include @@ -46,6 +50,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { +using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -100,7 +105,7 @@ Polygon2d createPolygon( Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; } @@ -124,6 +129,8 @@ 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"); + use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); + use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); @@ -172,6 +179,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( 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, "use_pointcloud_data", use_pointcloud_data_); + updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); @@ -298,13 +307,31 @@ bool AEB::fetchLatestData() return missing("ego velocity"); } - const auto pointcloud_ptr = sub_point_cloud_.takeData(); - if (!pointcloud_ptr) { - return missing("object pointcloud message"); + if (use_pointcloud_data_) { + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + + onPointCloud(pointcloud_ptr); + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + } else { + obstacle_ros_pointcloud_ptr_.reset(); } - onPointCloud(pointcloud_ptr); - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); + + if (use_predicted_object_data_) { + predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + if (!predicted_objects_ptr_) { + return missing("predicted objects"); + } + } else { + predicted_objects_ptr_.reset(); + } + + if (!obstacle_ros_pointcloud_ptr_ && !predicted_objects_ptr_) { + return missing("object detection method (pointcloud or predicted objects)"); } const auto imu_ptr = sub_imu_.takeData(); @@ -381,27 +408,33 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto & path, const colorTuple & debug_colors, const std::string & debug_ns, 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_); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + auto objects = std::invoke([&]() { + std::vector objects; + // Crop out Pointcloud using an extra wide ego path + if (use_pointcloud_data_) { + const auto expanded_ego_polys = + generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + createObjectDataUsingPointCloudClusters( + path, ego_polys, current_time, objects, filtered_objects); + } + if (use_predicted_object_data_) { + createObjectDataUsingPredictedObjects(path, ego_polys, objects); + } + return 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) { + const auto closest_object_point_itr = + std::min_element(objects.begin(), objects.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()) { + if (closest_object_point_itr == objects.end()) { return std::nullopt; } const auto closest_object_speed = (use_object_velocity_calculation_) @@ -419,8 +452,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) { 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); + this->get_clock()->now(), path, ego_polys, objects, 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()) @@ -462,9 +495,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // 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_); + 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; } @@ -577,6 +610,93 @@ std::vector AEB::generatePathFootprint( return polygons; } +void AEB::createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & object_data_vector) +{ + if (predicted_objects_ptr_->objects.empty()) return; + + const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; + const auto & objects = predicted_objects_ptr_->objects; + const auto & stamp = predicted_objects_ptr_->header.stamp; + + // Ego position + const auto current_p = [&]() { + const auto & first_point_of_path = ego_path.front(); + const auto & p = first_point_of_path.position; + return autoware_universe_utils::createPoint(p.x, p.y, p.z); + }(); + + auto get_object_tangent_velocity = + [&](const PredictedObject & predicted_object, const auto & obj_pose) { + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const auto obj_yaw = tf2::getYaw(obj_pose.orientation); + const auto obj_idx = autoware_motion_utils::findNearestIndex(ego_path, obj_pose.position); + const auto path_yaw = (current_ego_speed > 0.0) + ? tf2::getYaw(ego_path.at(obj_idx).orientation) + : tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI; + return obj_vel_norm * std::cos(obj_yaw - path_yaw); + }; + + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_objects_ptr_->header.frame_id, stamp, + 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; + } + + // Check which objects collide with the ego footprints + std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { + // get objects in base_link frame + const auto t_predicted_object = + utils::transformObjectFrame(predicted_object, transform_stamped); + const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = convertObjToPolygon(t_predicted_object); + const double obj_tangent_velocity = get_object_tangent_velocity(t_predicted_object, obj_pose); + + for (const auto & ego_poly : ego_polys) { + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(ego_poly, obj_poly, collision_points_bg); + if (collision_points_bg.empty()) continue; + + // Create an object for each intersection point + bool collision_points_added{false}; + for (const auto & collision_point : collision_points_bg) { + const auto obj_position = + autoware_universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + const double obj_arc_length = + autoware_motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if (std::isnan(obj_arc_length)) continue; + + // 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) ? 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; + obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0; + obj.distance_to_object = std::abs(dist_ego_to_object); + object_data_vector.push_back(obj); + collision_points_added = true; + } + // The ego polygons are in order, so the first intersection points found are the closest + // points. It is not necessary to continue iterating the ego polys for the same object. + if (collision_points_added) break; + } + }); +} + 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) diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp new file mode 100644 index 0000000000000..81f6afee31172 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -0,0 +1,150 @@ +// 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 + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_universe_utils::Point2d; +using autoware_universe_utils::Polygon2d; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; + +PredictedObject transformObjectFrame( + const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped) +{ + PredictedObject output = input; + const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; + const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; + const auto & pose = input.kinematics.initial_pose_with_covariance.pose; + + geometry_msgs::msg::Pose t_pose; + Vector3 t_linear_twist; + Vector3 t_angular_twist; + + tf2::doTransform(pose, t_pose, transform_stamped); + tf2::doTransform(linear_twist, t_linear_twist, transform_stamped); + tf2::doTransform(angular_twist, t_angular_twist, transform_stamped); + + output.kinematics.initial_pose_with_covariance.pose = t_pose; + output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist; + output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist; + return output; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} +} // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c561a8c1b9018..6aa70b4b464d4 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -148,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/velocity", "/vehicle/status/velocity_status"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), ( "~/input/predicted_trajectory", "/control/trajectory_follower/lateral/predicted_trajectory", From 647d9d5d99129cd2eacecc6a528fe942ef9a1dcc Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Sat, 22 Jun 2024 09:59:15 +0900 Subject: [PATCH 02/28] feat(autonomous_emergency_braking): add cluster min height for aeb (#7605) * add minimum cluster height threshold Signed-off-by: Daniel Sanchez * add update param option Signed-off-by: Daniel Sanchez * use param Signed-off-by: Daniel Sanchez * avoid the float check if cluster_surpasses_threshold_height is already true Signed-off-by: Daniel Sanchez * update README Signed-off-by: Daniel Sanchez * add cluster height description Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 60 +- .../autonomous_emergency_braking.param.yaml | 1 + .../image/closest-point.drawio.svg | 245 ++++++++ .../image/obstacle_filtering_2.drawio.svg | 575 +++++++----------- .../image/using-predicted-objects.drawio.svg | 114 ++++ .../autonomous_emergency_braking/node.hpp | 1 + .../src/node.cpp | 10 +- 7 files changed, 645 insertions(+), 361 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg create mode 100644 control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 2f3f5f08d4156..003611b83ae88 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -12,7 +12,9 @@ This module has following assumptions. - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. -- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses point cloud as obstacles. +- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles. + +- The AEBs target obstacles are 2D points that can be obtained from the input point cloud or by obtaining the intersection points between the predicted ego footprint path and a predicted object's shape. ### IMU path generation: steering angle vs IMU's angular velocity @@ -40,11 +42,13 @@ AEB has the following steps before it outputs the emergency stop signal. 2. Generate a predicted path of the ego vehicle. -3. Get target obstacles from the input point cloud. +3. Get target obstacles from the input point cloud and/or predicted object data. + +4. Estimate the closest obstacle speed. -4. Collision check with target obstacles. +5. Collision check with target obstacles. -5. Send emergency stop signals to `/diagnostics`. +6. Send emergency stop signals to `/diagnostics`. We give more details of each section below. @@ -58,7 +62,7 @@ We do not activate AEB module if it satisfies the following conditions. ### 2. Generate a predicted path of the ego vehicle -AEB generates a predicted path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: +AEB generates a predicted footprint path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: $$ x_{k+1} = x_k + v cos(\theta_k) dt \\ @@ -68,29 +72,47 @@ $$ where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance. -### 3. Get target obstacles from the input point cloud +On the other hand, if `use_predicted_trajectory` is set to true, the AEB module will use the predicted path from the MPC as a base to generate a footprint path. Both the IMU footprint path and the MPC footprint path can be used at the same time. + +### 3. Get target obstacles + +After generating the ego footprint path(s), the target obstacles are identified. There are two methods to find target obstacles: using the input point cloud, or using the predicted object information coming from perception modules. -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. +#### Pointcloud obstacle filtering -#### Rough filtering +The AEB module can filter the input pointcloud to find target obstacles with which the ego vehicle might collide. This method can be enable if the `use_pointcloud_data` parameter is set to true. The pointcloud obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. -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 + +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 rough filtering step is illustrated below. ![rough_filtering](./image/obstacle_filtering_1.drawio.svg) -#### Noise filtering with clustering and convex hulls +##### 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: . +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. Furthermore, each point in a cluster is compared against the `cluster_minimum_height` parameter, if no point inside a cluster has a height/z value greater than `cluster_minimum_height`, the whole cluster of points is 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 +##### Rigorous filtering -After Noise filtering, the module 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. 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. +After Noise filtering, the module 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) -#### Obstacle velocity estimation +#### Using predicted objects to get target obstacles + +If the `use_predicted_object_data` parameter is set to true, the AEB can use predicted object data coming from the perception modules, to get target obstacle points. This is done by obtaining the 2D intersection points between the ego's predicted footprint path and each of the predicted objects enveloping polygon or bounding box. + +![predicted_object_and_path_intersection](./image/using-predicted-objects.drawio.svg) + +### Finding the closest target obstacle + +Once all target obstacles have been identified, the AEB module chooses the point that is closest to the ego vehicle as the candidate for collision checking. Only the closest point is considered because RSS distance is used to judge if a collision will happen or not, and if the closest vertex to the ego is deemed to be safe from collision, the rest of the target obstacles will also be safe. + +![closest_object](./image/closest-point.drawio.svg) + +### 4. 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 relative speed using the following equations: @@ -110,7 +132,9 @@ Where $t_{1}$ and $t_{0}$ are the timestamps of the point clouds used to detect ![relative_speed](./image/object_relative_speed.drawio.svg) -Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: +Note that, when the closest obstacle/point comes from using predicted object data, $v_{norm}$ is calculated by directly computing the norm of the predicted object's velocity in the x and y axes. + +The velocity vector is then compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: $$ v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego} @@ -120,7 +144,9 @@ where $yaw_{diff}$ is the difference in yaw between the ego path and the displac Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity is set to 0 m/s. That is because the RSS distance calculation assumes the ego and the object move in the same direction and it cannot deal with negative velocities. -### 4. Collision check with target obstacles using RSS distance +The resulting estimated object speed is added to a queue of speeds with timestamps. The AEB then checks for expiration of past speed estimations and eliminates expired speed measurements from the queue, the object expiration is determined by checking if the time elapsed since the speed was first added to the queue is larger than the parameter `previous_obstacle_keep_time`. Finally, the median speed of the queue is calculated. The median speed will be used to calculate the RSS distance used for collision checking. + +### 5. Collision check with target obstacles using RSS distance In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as: @@ -132,7 +158,7 @@ where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ an ![rss_check](./image/rss_check.drawio.svg) -### 5. Send emergency stop signals to `/diagnostics` +### 6. Send emergency stop signals to `/diagnostics` If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state. diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 344d076197170..482ddc50766f8 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -28,6 +28,7 @@ # Point cloud clustering cluster_tolerance: 0.1 #[m] + cluster_minimum_height: 0.0 minimum_cluster_size: 10 maximum_cluster_size: 10000 diff --git a/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg b/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg new file mode 100644 index 0000000000000..154bba1fc9152 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ Find the closest target obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg index 1d12fe233ad8e..137872daebace 100644 --- a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg @@ -6,360 +6,249 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="616px" - height="534px" - viewBox="-0.5 -0.5 616 534" - content="<mxfile host="app.diagrams.net" modified="2024-06-13T07:23:40.787Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="VWopeiBdF3bKI_lRsSXP" version="24.5.4" type="google" scale="1" border="0"> <diagram name="Page-1" id="nTw8mlOSOdTxmldlJuHK"> <mxGraphModel dx="941" dy="648" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-1" value="" style="group;opacity=30;" vertex="1" connectable="0" parent="1"> <mxGeometry x="340" y="602" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-2" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-4" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-5" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-6" value="" style="group" vertex="1" connectable="0" parent="1"> <mxGeometry x="340" y="647" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-7" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-8" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-9" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-10" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" vertex="1" parent="1"> <mxGeometry x="180" y="618" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-11" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="456.7649999999999" y="677.5" as="targetPoint" /> <mxPoint x="350.0000000000002" y="677.4867690685733" as="sourcePoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-12" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="466.34" y="655" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-13" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="464" y="671" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-14" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="468.34000000000003" y="685" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-15" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="480" y="655" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-16" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="472" y="665" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-17" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" vertex="1" parent="1"> <mxGeometry x="235.47000000000003" y="651.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-18" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="482" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="478.34000000000003" y="695" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-20" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="492" y="683" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-21" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="392.66999999999996" y="782" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-22" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="468" y="676" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-23" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="683" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="481" y="684" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-25" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="473" y="689" as="sourcePoint" /> <mxPoint x="468.24800000000005" y="675.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-26" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="699" as="sourcePoint" /> <mxPoint x="473.068" y="689.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-27" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="491" y="709" as="sourcePoint" /> <mxPoint x="482.068" y="699.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-28" value="" style="group;opacity=30;" vertex="1" connectable="0" parent="1"> <mxGeometry x="334.84" y="872" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-29" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-30" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-31" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-32" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-33" value="" style="group" vertex="1" connectable="0" parent="1"> <mxGeometry x="334.84" y="917" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-34" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-35" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-36" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-37" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" vertex="1" parent="1"> <mxGeometry x="174.84" y="888" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-38" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="461.17999999999995" y="925" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-39" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e3c800;strokeColor=#B09500;fontColor=#000000;" vertex="1" parent="1"> <mxGeometry x="458.84" y="941" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-40" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="463.18" y="955" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-41" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="474.84" y="925" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-42" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" vertex="1" parent="1"> <mxGeometry x="230.31000000000003" y="921.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-43" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="473.18" y="965" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-44" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="486.84" y="953" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-45" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="387.50999999999993" y="1052" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-46" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="462.84" y="946" as="sourcePoint" /> <mxPoint x="478.84" y="929" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-47" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-44"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="476.84" y="953" as="sourcePoint" /> <mxPoint x="478.84" y="929" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-48" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="467.84" y="959" as="sourcePoint" /> <mxPoint x="463.088" y="945.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-49" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="476.84" y="969" as="sourcePoint" /> <mxPoint x="467.90799999999996" y="959.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-50" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-44"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="485.84" y="979" as="sourcePoint" /> <mxPoint x="476.90799999999996" y="969.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-51" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.008;exitY=0.543;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-42" target="ty0ni0eQC3Mwg9WxKvAH-39"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="353.5" y="949" as="sourcePoint" /> <mxPoint x="403.5" y="899" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-52" value="&lt;font style=&quot;font-size: 14px;&quot;&gt;Closest Point&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;fontColor=#000000;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-51"> <mxGeometry x="-0.084" y="1" relative="1" as="geometry"> <mxPoint x="3" y="-7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-53" value="Filter out points outside clusters and get a 2D convex hull of each cluster" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" vertex="1" parent="1"> <mxGeometry x="159" y="564" width="600" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-54" value="Find the closest vertex of the convex hull(s)" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" vertex="1" parent="1"> <mxGeometry x="301" y="832" width="370" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-55" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="535" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-56" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.x;fillColor=#FF0000;opacity=40;" vertex="1" parent="1"> <mxGeometry x="529" y="668" width="20" height="19" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + height="265px" + viewBox="-0.5 -0.5 616 265" + content="<mxfile host="app.diagrams.net" modified="2024-06-21T02:42:01.489Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="3pgNd0JrTgvgM2V0BwGE" version="24.6.0" type="google" scale="1" border="0"> <diagram name="Page-1" id="nTw8mlOSOdTxmldlJuHK"> <mxGraphModel dx="1137" dy="783" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-1" value="" style="group;opacity=30;" parent="1" connectable="0" vertex="1"> <mxGeometry x="340" y="602" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-2" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" parent="ty0ni0eQC3Mwg9WxKvAH-1" connectable="0" vertex="1"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-4" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-5" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-6" value="" style="group" parent="1" connectable="0" vertex="1"> <mxGeometry x="340" y="647" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-7" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-8" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-9" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-10" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" parent="1" vertex="1"> <mxGeometry x="180" y="618" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-11" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="1" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="456.7649999999999" y="677.5" as="targetPoint" /> <mxPoint x="350.0000000000002" y="677.4867690685733" as="sourcePoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-12" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,PD94bWwgdmVyc2lvbj0iMS4wIiBlbmNvZGluZz0iVVRGLTgiIHN0YW5kYWxvbmU9Im5vIj8+CjwhLS0gQ3JlYXRlZCB3aXRoIElua3NjYXBlIChodHRwOi8vd3d3Lmlua3NjYXBlLm9yZy8pIC0tPgoKPHN2ZwogICB3aWR0aD0iNy4zMDU0NjA5bW0iCiAgIGhlaWdodD0iMy4xODI4MzIybW0iCiAgIHZpZXdCb3g9IjAgMCA3LjMwNTQ2MDkgMy4xODI4MzIyIgogICB2ZXJzaW9uPSIxLjEiCiAgIGlkPSJzdmcxIgogICB4bWw6c3BhY2U9InByZXNlcnZlIgogICBpbmtzY2FwZTp2ZXJzaW9uPSIxLjMgKDE6MS4zKzIwMjMwNzIzMTQ1OSswZTE1MGVkNmM0KSIKICAgc29kaXBvZGk6ZG9jbmFtZT0iYmVoYXZpb3JfcGF0aF9wbGFubmVyLnN2ZyIKICAgeG1sbnM6aW5rc2NhcGU9Imh0dHA6Ly93d3cuaW5rc2NhcGUub3JnL25hbWVzcGFjZXMvaW5rc2NhcGUiCiAgIHhtbG5zOnNvZGlwb2RpPSJodHRwOi8vc29kaXBvZGkuc291cmNlZm9yZ2UubmV0L0RURC9zb2RpcG9kaS0wLmR0ZCIKICAgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIgogICB4bWxuczpzdmc9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIj48c29kaXBvZGk6bmFtZWR2aWV3CiAgICAgaWQ9Im5hbWVkdmlldzEiCiAgICAgcGFnZWNvbG9yPSIjZmZmZmZmIgogICAgIGJvcmRlcmNvbG9yPSIjMDAwMDAwIgogICAgIGJvcmRlcm9wYWNpdHk9IjAuMjUiCiAgICAgaW5rc2NhcGU6c2hvd3BhZ2VzaGFkb3c9IjIiCiAgICAgaW5rc2NhcGU6cGFnZW9wYWNpdHk9IjAuMCIKICAgICBpbmtzY2FwZTpwYWdlY2hlY2tlcmJvYXJkPSIwIgogICAgIGlua3NjYXBlOmRlc2tjb2xvcj0iI2QxZDFkMSIKICAgICBpbmtzY2FwZTpkb2N1bWVudC11bml0cz0ibW0iCiAgICAgaW5rc2NhcGU6em9vbT0iMTkuMTA5NTAxIgogICAgIGlua3NjYXBlOmN4PSIxNzkuODMyMDEiCiAgICAgaW5rc2NhcGU6Y3k9IjQ4MC40NDE2MyIKICAgICBpbmtzY2FwZTp3aW5kb3ctd2lkdGg9IjM3NzAiCiAgICAgaW5rc2NhcGU6d2luZG93LWhlaWdodD0iMjA5NiIKICAgICBpbmtzY2FwZTp3aW5kb3cteD0iNzAiCiAgICAgaW5rc2NhcGU6d2luZG93LXk9IjI3IgogICAgIGlua3NjYXBlOndpbmRvdy1tYXhpbWl6ZWQ9IjEiCiAgICAgaW5rc2NhcGU6Y3VycmVudC1sYXllcj0ibGF5ZXIxIiAvPjxkZWZzCiAgICAgaWQ9ImRlZnMxIiAvPjxnCiAgICAgaW5rc2NhcGU6bGFiZWw9IkxheWVyIDEiCiAgICAgaW5rc2NhcGU6Z3JvdXBtb2RlPSJsYXllciIKICAgICBpZD0ibGF5ZXIxIgogICAgIHRyYW5zZm9ybT0idHJhbnNsYXRlKC01MS4zMjMzMjMsLTEzMi4wNTM0MikiPjxyZWN0CiAgICAgICBzdHlsZT0iZmlsbDojZjhjZWNjO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTojMDAwMDAwO3N0cm9rZS13aWR0aDowLjE1O3N0cm9rZS1kYXNoYXJyYXk6bm9uZSIKICAgICAgIGlkPSJyZWN0MSIKICAgICAgIHdpZHRoPSI3LjE1NTQ2MzciCiAgICAgICBoZWlnaHQ9IjMuMDMyODMxNCIKICAgICAgIHg9IjUxLjM5ODMyMyIKICAgICAgIHk9IjEzMi4xMjg0MiIKICAgICAgIHJ5PSIwLjkzNTU2ODM5IiAvPjxwYXRoCiAgICAgICBpZD0icGF0aDkiCiAgICAgICBzdHlsZT0iZmlsbDojZjhjZWNjO2ZpbGwtb3BhY2l0eToxO3N0cm9rZTojMDAwMDAwO3N0cm9rZS13aWR0aDowLjE1O3N0cm9rZS1kYXNoYXJyYXk6bm9uZSIKICAgICAgIGQ9Im0gNTYuNjM1MjgyLDEzNC43OTk4NCAtMS4xNDEzNjUsLTAuMjQ1NSBtIDEuMTQxMzY1LC0yLjA1ODEgLTEuMTQxMzY1LDAuMjQ1NSBtIC0yLjYzNzE1NiwyLjA4NDY3IDEuMjM4Mzg3LC0wLjI5ODYzIG0gLTEuMjM4Mzg3LC0yLjA1ODExIDEuMjM4Mzg3LDAuMjk4NjQgbSAwLjczOTM5MSwxLjc3MDY4IHYgMC4zNTcyNyBtIDAsLTIuNDkzOTYgdiAwLjM1NzI3IG0gLTAuNzgyMjUxLC0wLjAxNDEgaCAxLjQ1ODQwMSB2IDEuODA3ODkgaCAtMS40NTg0MDEgeiBtIC0wLjg0MDE1NCwtMC4zMzA4MyBoIDIuOTE2Mzk4IGMgMC41ODkwMTksMCAxLjA2MzIxMiwwLjMzOTM5IDEuMDYzMjEyLDAuNzYwOTYgdiAwLjk0NDg4IGMgMCwwLjQyMTU3IC0wLjQ3NDE5MywwLjc2MDk2IC0xLjA2MzIxMiwwLjc2MDk2IGggLTIuOTE2Mzk4IGMgLTAuNTg5MDE5LDAgLTEuMDYzMjEyLC0wLjMzOTM5IC0xLjA2MzIxMiwtMC43NjA5NiB2IC0wLjk0NDg4IGMgMCwtMC40MjE1NyAwLjQ3NDE5MywtMC43NjA5NiAxLjA2MzIxMiwtMC43NjA5NiB6IiAvPjwvZz48L3N2Zz4K;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" parent="1" vertex="1"> <mxGeometry x="466.34" y="655" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-13" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="464" y="671" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-14" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="468.34000000000003" y="685" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-15" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="480" y="655" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-16" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="472" y="665" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-17" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" parent="1" vertex="1"> <mxGeometry x="235.47000000000003" y="651.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-18" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="482" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="478.34000000000003" y="695" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-20" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="492" y="683" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-21" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" parent="1" vertex="1"> <mxGeometry x="392.66999999999996" y="782" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-22" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="468" y="676" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-23" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="683" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="481" y="684" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-25" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="473" y="689" as="sourcePoint" /> <mxPoint x="468.24800000000005" y="675.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-26" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="699" as="sourcePoint" /> <mxPoint x="473.068" y="689.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-27" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="491" y="709" as="sourcePoint" /> <mxPoint x="482.068" y="699.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-53" value="Filter out points outside clusters and get a 2D convex hull of each cluster" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" parent="1" vertex="1"> <mxGeometry x="159" y="564" width="600" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-55" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="535" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-56" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.x;fillColor=#FF0000;opacity=40;" parent="1" vertex="1"> <mxGeometry x="529" y="668" width="20" height="19" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " style="background-color: rgb(255, 255, 255);" > - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+ + -
-
- Closest Point -
-
-
- - - - - - - - - - - - -
-
-
- Filter out points outside clusters and get a 2D convex hull of each cluster -
-
-
- - - - - - - - - - - - -
-
-
- Find the closest vertex of the convex hull(s) -
-
-
- - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Filter out points outside clusters and get a 2D convex hull of each cluster +
+
+
+ + + + + + + + + + + + + + + + - - - - - - diff --git a/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg b/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg new file mode 100644 index 0000000000000..244b609c7f009 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego path footprint and object polygon intersection points +
+
+
+ + + + + + + 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 1054ad8f57c1b..2ff9398a241fa 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 @@ -347,6 +347,7 @@ class AEB : public rclcpp::Node double a_ego_min_; double a_obj_min_; double cluster_tolerance_; + double cluster_minimum_height_; int minimum_cluster_size_; int maximum_cluster_size_; double imu_prediction_time_horizon_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index a2d94c06e9fe1..2252a1bc205f7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -147,6 +147,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) a_obj_min_ = declare_parameter("a_obj_min"); cluster_tolerance_ = declare_parameter("cluster_tolerance"); + cluster_minimum_height_ = declare_parameter("cluster_minimum_height"); minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); @@ -198,6 +199,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "a_obj_min", a_obj_min_); updateParam(parameters, "cluster_tolerance", cluster_tolerance_); + updateParam(parameters, "cluster_minimum_height", cluster_minimum_height_); updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); @@ -725,9 +727,15 @@ void AEB::createObjectDataUsingPointCloudClusters( PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); + bool cluster_surpasses_threshold_height{false}; for (const auto & index : indices.indices) { - cluster->push_back((*obstacle_points_ptr)[index]); + const auto & p = (*obstacle_points_ptr)[index]; + cluster_surpasses_threshold_height = (cluster_surpasses_threshold_height) + ? cluster_surpasses_threshold_height + : (p.z > cluster_minimum_height_); + cluster->push_back(p); } + if (!cluster_surpasses_threshold_height) continue; // Make a 2d convex hull for the objects pcl::ConvexHull hull; hull.setDimension(2); From cab9c6aed5c1c2d469cd7829243eccd9bf198f1c Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 28 Jun 2024 15:52:24 +0900 Subject: [PATCH 03/28] fix(autonomous_emergency_braking): aeb strange mpc polygon (#7740) change resize to reserve Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 2252a1bc205f7..c24ae22bcf0e5 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -588,11 +588,11 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) // create path Path path; - path.resize(predicted_traj.points.size()); + path.reserve(predicted_traj.points.size()); for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); - path.at(i) = map_pose; + path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { break; From 03049a74ccb44e32624875282d4bf702aae295ce Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 28 Jun 2024 15:53:05 +0900 Subject: [PATCH 04/28] feat(autonomous_emergency_braking): aeb add support negative speeds (#7707) * add support for negative speeds Signed-off-by: Daniel Sanchez * remove negative speed check for predicted obj Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 6 ++---- .../autonomous_emergency_braking/node.hpp | 4 ++-- .../src/node.cpp | 21 ++++++++++++++----- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 003611b83ae88..f040235f3f052 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -142,7 +142,7 @@ $$ 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 current 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). -Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity is set to 0 m/s. That is because the RSS distance calculation assumes the ego and the object move in the same direction and it cannot deal with negative velocities. +Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity will be negative, which will reduce the rss distance on the next step. The resulting estimated object speed is added to a queue of speeds with timestamps. The AEB then checks for expiration of past speed estimations and eliminates expired speed measurements from the queue, the object expiration is determined by checking if the time elapsed since the speed was first added to the queue is larger than the parameter `previous_obstacle_keep_time`. Finally, the median speed of the queue is calculated. The median speed will be used to calculate the RSS distance used for collision checking. @@ -151,7 +151,7 @@ The resulting estimated object speed is added to a queue of speeds with timestam In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as: $$ -d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset +d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) -(sign(v_{obj})) * v_{obj}^2/(2*a_{obj_{min}}) + offset $$ where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture. @@ -221,6 +221,4 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t - The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle. -- Currently, the module calculates thee closest object velocity if it goes in the same direction as the ego vehicle, otherwise the velocity is set to 0 m/s since RSS distance calculation does not use negative velocities. - ![aeb_range](./image/range.drawio.svg) 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 2ff9398a241fa..0f69189de611d 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 @@ -181,7 +181,7 @@ class CollisionDataKeeper const ObjectData & closest_object, const Path & path, const double current_ego_speed) { // in case the object comes from predicted objects info, we reuse the speed. - if (closest_object.velocity > 0.0) { + if (std::abs(closest_object.velocity) > std::numeric_limits::epsilon()) { this->setPreviousObjectData(closest_object); this->updateVelocityHistory(closest_object.velocity, closest_object.stamp); return this->getMedianObstacleVelocity(); @@ -218,7 +218,7 @@ class CollisionDataKeeper p_vel * std::cos(p_yaw - traj_yaw) + std::abs(current_ego_speed); // Current RSS distance calculation does not account for negative velocities - return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; + return estimated_velocity; }); if (!estimated_velocity_opt.has_value()) { diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index c24ae22bcf0e5..69f32313415f9 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -39,6 +39,9 @@ #include #include #include + +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -508,9 +511,17 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object { const double & obj_v = closest_object.velocity; const double & t = t_response_; - const double rss_dist = std::abs(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_; + + const double rss_dist = std::invoke([&]() { + const double pre_braking_covered_distance = std::abs(current_v) * t; + const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_)); + const double ego_stopping_distance = pre_braking_covered_distance + braking_distance; + const double obj_braking_distance = (obj_v > 0.0) + ? -(obj_v * obj_v) / (2 * std::fabs(a_obj_min_)) + : (obj_v * obj_v) / (2 * std::fabs(a_obj_min_)); + return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; + }); + if (closest_object.distance_to_object < rss_dist) { // collision happens ObjectData collision_data = closest_object; @@ -538,7 +549,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) return path; } - constexpr double epsilon = 1e-6; + constexpr double epsilon = std::numeric_limits::epsilon(); const double & dt = imu_prediction_time_interval_; const double & horizon = imu_prediction_time_horizon_; for (double t = 0.0; t < horizon + epsilon; t += dt) { @@ -687,7 +698,7 @@ void AEB::createObjectDataUsingPredictedObjects( ObjectData obj; obj.stamp = stamp; obj.position = obj_position; - obj.velocity = (obj_tangent_velocity > 0.0) ? obj_tangent_velocity : 0.0; + obj.velocity = obj_tangent_velocity; obj.distance_to_object = std::abs(dist_ego_to_object); object_data_vector.push_back(obj); collision_points_added = true; From a1957681cc77a5e016a51d8b876fbca2df59c543 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 28 Jun 2024 15:53:19 +0900 Subject: [PATCH 05/28] chore(autonomous_emergency_braking): apply clangd suggestions to aeb (#7703) * apply clangd suggestions Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .github/CODEOWNERS | 2 +- .../autoware/autonomous_emergency_braking/node.hpp | 14 +++++++------- .../autonomous_emergency_braking/utils.hpp | 3 ++- .../src/node.cpp | 1 + .../src/utils.cpp | 11 ++++++----- 5 files changed, 17 insertions(+), 14 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index df8673cd68724..0e3032ddebc26 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -44,7 +44,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp 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 0f69189de611d..a9928bd4627b8 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 @@ -60,7 +60,6 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; @@ -120,12 +119,12 @@ class CollisionDataKeeper return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } - ObjectData get() const + [[nodiscard]] ObjectData get() const { return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); } - ObjectData getPreviousObjectData() const + [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } @@ -156,20 +155,21 @@ class CollisionDataKeeper }), obstacle_velocity_history_.end()); obstacle_velocity_history_.emplace_back( - std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + current_object_velocity, current_object_velocity_time_stamp); } - std::optional getMedianObstacleVelocity() const + [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; std::vector raw_velocities; + raw_velocities.reserve(obstacle_velocity_history_.size()); 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; + : (raw_velocities.size()) / 2; + const size_t med2 = (raw_velocities.size()) / 2; 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()); diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index b046acd9153e9..ef419942aaa02 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -37,6 +37,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_universe_utils::Point2d; @@ -51,7 +52,7 @@ using geometry_msgs::msg::TransformStamped; * @param transform_stamped the tf2 transform */ PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped); + const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped); /** * @brief Get the predicted objects polygon as a geometry polygon diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 69f32313415f9..819b252ae3831 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -54,6 +54,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; +using autoware::universe_utils::Point2d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 81f6afee31172..d6f21916a238f 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -16,6 +16,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { +using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_universe_utils::Point2d; @@ -26,7 +27,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Vector3; PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped) + const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped) { PredictedObject output = input; const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; @@ -74,12 +75,12 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( const double obj_x = current_pose.position.x; const double obj_y = current_pose.position.y; - constexpr int N = 20; + constexpr int n = 20; const double r = obj_shape.dimensions.x / 2; - object_polygon.outer().reserve(N + 1); - for (int i = 0; i < N; ++i) { + object_polygon.outer().reserve(n + 1); + for (int i = 0; i < n; ++i) { object_polygon.outer().emplace_back( - obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i)); } object_polygon.outer().push_back(object_polygon.outer().front()); From 594fc7b1822d151642f9bd274f7e3f49aec726a7 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 9 Jul 2024 18:46:05 +0900 Subject: [PATCH 06/28] feat(autonomous_emergency_braking): add virtual stop wall to aeb (#7894) * add virtual stop wall to aeb Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez * add uppercase Signed-off-by: Daniel Sanchez * use motion utils function instead of shiftPose Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 1 + .../autonomous_emergency_braking.param.yaml | 1 + .../autonomous_emergency_braking/node.hpp | 1 + .../autonomous_emergency_braking/utils.hpp | 2 - .../package.xml | 1 + .../src/node.cpp | 38 ++++++++++++++++++- 6 files changed, 40 insertions(+), 4 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f040235f3f052..db27ca1b4f699 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -192,6 +192,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | Name | Unit | Type | Description | Default value | | :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | | publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 482ddc50766f8..25db9448f758b 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -14,6 +14,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 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 a9928bd4627b8..ebd1e63fd73ec 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 @@ -329,6 +329,7 @@ class AEB : public rclcpp::Node // member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index ef419942aaa02..2d3ed669e86bb 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..0c64b9498486a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 819b252ae3831..d4aed3fa6053f 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -131,6 +134,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); @@ -182,6 +186,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); @@ -378,7 +383,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) 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); + if (publish_debug_markers_) { + addCollisionMarker(data, debug_markers); + } } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -455,7 +462,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }); // Add debug markers - { + if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, @@ -896,6 +903,33 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); + + const auto ego_map_pose = std::invoke([this]() -> std::optional { + geometry_msgs::msg::TransformStamped tf_current_pose; + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return std::nullopt; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( + ego_front_pose, "autonomous_emergency_braking", this->now(), 0); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + } } } // namespace autoware::motion::control::autonomous_emergency_braking From 36b06a522f051ecad54bcc58e65a9db30121eefb Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 10:51:02 +0900 Subject: [PATCH 07/28] feat(autonomous_emergency_braking): add info marker and override for state (#8312) add info marker and override for state Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking.param.yaml | 1 + .../autonomous_emergency_braking/node.hpp | 6 ++- .../src/node.cpp | 41 ++++++++++++------- 3 files changed, 33 insertions(+), 15 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 25db9448f758b..f13e95c6db8e5 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -6,6 +6,7 @@ use_pointcloud_data: true use_predicted_object_data: true use_object_velocity_calculation: true + check_autoware_state: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 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 ebd1e63fd73ec..a85a0f890f0cd 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 @@ -260,7 +260,8 @@ class AEB : public rclcpp::Node this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; - rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + rclcpp::Publisher::SharedPtr debug_marker_publisher_; + rclcpp::Publisher::SharedPtr info_marker_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; @@ -308,6 +309,8 @@ class AEB : public rclcpp::Node void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + void addVirtualStopWallMarker(MarkerArray & markers); + std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); @@ -335,6 +338,7 @@ class AEB : public rclcpp::Node bool use_pointcloud_data_; bool use_predicted_object_data_; bool use_object_velocity_calculation_; + bool check_autoware_state_; double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index d4aed3fa6053f..e2a81abc724a8 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -125,7 +125,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) { pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); - debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); + info_marker_publisher_ = this->create_publisher("~/info/markers", 1); } // Diagnostics { @@ -140,6 +141,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); + check_autoware_state_ = declare_parameter("check_autoware_state"); 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_ = @@ -193,6 +195,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); + updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( @@ -363,7 +366,7 @@ bool AEB::fetchLatestData() } autoware_state_ = sub_autoware_state_.takeData(); - if (!autoware_state_) { + if (check_autoware_state_ && !autoware_state_) { return missing("autoware_state"); } @@ -373,6 +376,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; + MarkerArray info_markers; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -386,6 +390,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) if (publish_debug_markers_) { addCollisionMarker(data, debug_markers); } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -393,7 +398,8 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); + debug_marker_publisher_->publish(debug_markers); + info_marker_publisher_->publish(info_markers); } bool AEB::checkCollision(MarkerArray & debug_markers) @@ -406,7 +412,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } // if not driving, disable aeb - if (autoware_state_->state != AutowareState::DRIVING) { + if (check_autoware_state_ && autoware_state_->state != AutowareState::DRIVING) { return false; } @@ -890,20 +896,17 @@ void AEB::addMarker( "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 - std::abs(ego_velocity)) + - " [m/s]"; + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object distance to ego: " + std::to_string(obj.distance_to_object) + " [m]\n"; + closest_object_velocity_marker_array.text += + "RSS distance: " + std::to_string(obj.rss) + " [m]"; debug_markers.markers.push_back(closest_object_velocity_marker_array); } } -void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +void AEB::addVirtualStopWallMarker(MarkerArray & markers) { - auto point_marker = autoware::universe_utils::createDefaultMarker( - "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); - point_marker.pose.position = data.position; - debug_markers.markers.push_back(point_marker); - const auto ego_map_pose = std::invoke([this]() -> std::optional { geometry_msgs::msg::TransformStamped tf_current_pose; geometry_msgs::msg::Pose p; @@ -928,10 +931,20 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( ego_front_pose, "autonomous_emergency_braking", this->now(), 0); - autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers); } } +void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +{ + auto point_marker = autoware::universe_utils::createDefaultMarker( + "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + point_marker.pose.position = data.position; + debug_markers.markers.push_back(point_marker); +} + } // namespace autoware::motion::control::autonomous_emergency_braking #include From 1ee945d50a5c5b5bcd43b66e85f6136928de867e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 14:55:30 +0900 Subject: [PATCH 08/28] docs(autonomous_emergency_braking): update readme for new param (#8330) update readme for new param Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index db27ca1b4f699..1382a24d98b0a 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -197,6 +197,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | | use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | | detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | | detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | | voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | From 20ed2547214901ed66fc925d1332d15794727a2d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 18:14:26 +0900 Subject: [PATCH 09/28] feat(autonomous_emergency_braking): add some tests to aeb (#8126) * add initial tests Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * more tests Signed-off-by: Daniel Sanchez * WIP add publishing and test subscription Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * fix lint cmake Signed-off-by: Daniel Sanchez * WIP tf topic Signed-off-by: Daniel Sanchez * Revert "WIP tf topic" This reverts commit b5ef11b499e719b2cdbe0464bd7de7778de54e76. Signed-off-by: Daniel Sanchez * add path crop test Signed-off-by: Daniel Sanchez * add test for transform object Signed-off-by: Daniel Sanchez * add briefs Signed-off-by: Daniel Sanchez * delete repeated test Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../CMakeLists.txt | 7 +- .../autonomous_emergency_braking/node.hpp | 192 ++++++++++- ...re_autonomous_emergency_braking.launch.xml | 1 - .../package.xml | 8 +- .../src/node.cpp | 3 +- .../src/utils.cpp | 3 + .../test/test.cpp | 326 ++++++++++++++++++ .../test/test.hpp | 116 +++++++ 8 files changed, 642 insertions(+), 14 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/test/test.cpp create mode 100644 control/autoware_autonomous_emergency_braking/test/test.hpp diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 85290c6c66730..9f7a64c18da9a 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -30,8 +30,11 @@ rclcpp_components_register_node(${AEB_NODE} ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_aeb + test/test.cpp) + +target_link_libraries(test_aeb ${AEB_NODE}) + endif() ament_auto_package( 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 a85a0f890f0cd..229ea49d89102 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 @@ -71,6 +71,9 @@ using Vector3 = geometry_msgs::msg::Vector3; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +/** + * @brief Struct to store object data + */ struct ObjectData { rclcpp::Time stamp; @@ -80,22 +83,44 @@ struct ObjectData double distance_to_object{0.0}; }; +/** + * @brief Class to manage collision data + */ class CollisionDataKeeper { public: + /** + * @brief Constructor for CollisionDataKeeper + * @param clock Shared pointer to the clock + */ explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + /** + * @brief Set timeout values for collision and obstacle data + * @param collision_keep_time Time to keep collision data + * @param previous_obstacle_keep_time Time to keep previous obstacle data + */ 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; } + /** + * @brief Get timeout values for collision and obstacle data + * @return Pair of collision and obstacle data timeout values + */ std::pair getTimeout() { return {collision_keep_time_, previous_obstacle_keep_time_}; } + /** + * @brief Check if object data has expired + * @param data Optional reference to the object data + * @param timeout Timeout value to check against + * @return True if object data has expired, false otherwise + */ bool checkObjectDataExpired(std::optional & data, const double timeout) { if (!data.has_value()) return true; @@ -109,38 +134,70 @@ class CollisionDataKeeper return false; } + /** + * @brief Check if collision data has expired + * @return True if collision data has expired, false otherwise + */ bool checkCollisionExpired() { return this->checkObjectDataExpired(closest_object_, collision_keep_time_); } + /** + * @brief Check if previous object data has expired + * @return True if previous object data has expired, false otherwise + */ bool checkPreviousObjectDataExpired() { return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } + /** + * @brief Get the closest object data + * @return Object data of the closest object + */ [[nodiscard]] ObjectData get() const { return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); } + /** + * @brief Get the previous closest object data + * @return Object data of the previous closest object + */ [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } + /** + * @brief Set collision data + * @param data Object data to set + */ void setCollisionData(const ObjectData & data) { closest_object_ = std::make_optional(data); } + /** + * @brief Set previous object data + * @param data Object data to set + */ void setPreviousObjectData(const ObjectData & data) { prev_closest_object_ = std::make_optional(data); } + /** + * @brief Reset the obstacle velocity history + */ void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + /** + * @brief Update the velocity history with current object velocity + * @param current_object_velocity Current object velocity + * @param current_object_velocity_time_stamp Timestamp of the current object velocity + */ void updateVelocityHistory( const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) { @@ -158,6 +215,10 @@ class CollisionDataKeeper current_object_velocity, current_object_velocity_time_stamp); } + /** + * @brief Get the median obstacle velocity from history + * @return Optional median obstacle velocity + */ [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; @@ -177,6 +238,13 @@ class CollisionDataKeeper return (vel1 + vel2) / 2.0; } + /** + * @brief Calculate object speed from velocity history + * @param closest_object Closest object data + * @param path Ego vehicle path + * @param current_ego_speed Current ego vehicle speed + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { @@ -241,9 +309,16 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +/** + * @brief Autonomous Emergency Braking (AEB) node + */ class AEB : public rclcpp::Node { public: + /** + * @brief Constructor for AEB + * @param node_options Options for the node + */ explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber @@ -267,53 +342,156 @@ class AEB : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // callback + /** + * @brief Callback for point cloud messages + * @param input_msg Shared pointer to the point cloud message + */ void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + /** + * @brief Callback for IMU messages + * @param input_msg Shared pointer to the IMU message + */ void onImu(const Imu::ConstSharedPtr input_msg); + + /** + * @brief Timer callback function + */ void onTimer(); + + /** + * @brief Callback for parameter updates + * @param parameters Vector of updated parameters + * @return Set parameters result + */ rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); + /** + * @brief Fetch the latest data from subscribers + * @return True if data fetch was successful, false otherwise + */ bool fetchLatestData(); - // main function + /** + * @brief Diagnostic check for collisions + * @param stat Diagnostic status wrapper + */ void onCheckCollision(DiagnosticStatusWrapper & stat); + + /** + * @brief Check for collisions + * @param debug_markers Marker array for debugging + * @return True if a collision is detected, false otherwise + */ bool checkCollision(MarkerArray & debug_markers); + + /** + * @brief Check if there is a collision with the closest object + * @param current_v Current velocity of the ego vehicle + * @param closest_object Data of the closest object + * @return True if a collision is detected, false otherwise + */ bool hasCollision(const double current_v, const ObjectData & closest_object); + /** + * @brief Generate the ego vehicle path + * @param curr_v Current velocity of the ego vehicle + * @param curr_w Current angular velocity of the ego vehicle + * @return Generated ego path + */ Path generateEgoPath(const double curr_v, const double curr_w); + + /** + * @brief Generate the ego vehicle path from the predicted trajectory + * @param predicted_traj Predicted trajectory of the ego vehicle + * @return Optional generated ego path + */ std::optional generateEgoPath(const Trajectory & predicted_traj); + + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @return Vector of polygons representing the path footprint + */ std::vector generatePathFootprint(const Path & path, const double extra_width_margin); + /** + * @brief Create object data using point cloud clusters + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param stamp Timestamp of the data + * @param objects Vector to store the created object data + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + */ void createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + /** + * @brief Create object data using predicted objects + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param objects Vector to store the created object data + */ void createObjectDataUsingPredictedObjects( const Path & ego_path, const std::vector & ego_polys, std::vector & objects); + /** + * @brief Crop the point cloud with the ego vehicle footprint path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param filtered_objects Pointer to the filtered point cloud of obstacles + */ 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); - + /** + * @brief Add a marker for debugging + * @param current_time Current time + * @param path Ego vehicle path + * @param polygons Polygons representing the ego vehicle footprint + * @param objects Vector of object data + * @param closest_object Optional data of the closest object + * @param color_r Red color component + * @param color_g Green color component + * @param color_b Blue color component + * @param color_a Alpha (transparency) component + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, 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); + /** + * @brief Add a collision marker for debugging + * @param data Data of the collision object + * @param debug_markers Marker array for debugging + */ void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + /** + * @brief Add an info marker stop wall in front of the ego vehicle + * @param markers Data of the closest object + */ void addVirtualStopWallMarker(MarkerArray & markers); + /** + * @brief Calculate object speed from history + * @param closest_object Data of the closest object + * @param path Ego vehicle path + * @param current_ego_speed Current speed of the ego vehicle + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); + // Member variables PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; @@ -330,7 +508,7 @@ class AEB : public rclcpp::Node // diag Updater updater_{this}; - // member variables + // Member variables bool publish_debug_pointcloud_; bool publish_debug_markers_; bool use_predicted_trajectory_; diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index fe0df41ca89c7..34d0492799577 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -3,7 +3,6 @@ - diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 0c64b9498486a..248d3689072fa 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -14,11 +14,16 @@ ament_cmake autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils autoware_control_msgs autoware_motion_utils autoware_planning_msgs autoware_system_msgs + autoware_test_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -38,9 +43,6 @@ tf2_ros visualization_msgs - ament_lint_auto - autoware_lint_common - ament_cmake diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index e2a81abc724a8..e8281a2adae46 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -507,7 +507,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }; // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); // evaluate if there is a collision for both paths const bool has_collision = has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index d6f21916a238f..0267ee7f6a8d8 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -51,6 +51,9 @@ PredictedObject transformObjectFrame( Polygon2d convertPolygonObjectToGeometryPolygon( const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { + if (obj_shape.footprint.points.empty()) { + return {}; + } Polygon2d object_polygon; tf2::Transform tf_map2obj; fromMsg(current_pose, tf_map2obj); diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp new file mode 100644 index 0000000000000..42054e718d09c --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -0,0 +1,326 @@ +// Copyright 2024 TIER IV +// +// 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 "test.hpp" + +#include "autoware/autonomous_emergency_braking/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::Header; + +Header get_header(const char * const frame_id, rclcpp::Time t) +{ + std_msgs::msg::Header header; + header.stamp = t; + header.frame_id = frame_id; + return header; +}; + +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z) +{ + Imu imu_msg; + imu_msg.header = header; + imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + imu_msg.angular_velocity.z = angular_velocity_z; + imu_msg.linear_acceleration.x = ax; + imu_msg.linear_acceleration.y = ay; + return imu_msg; +}; + +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate) +{ + VelocityReport velocity_msg; + velocity_msg.header = header; + velocity_msg.lateral_velocity = lat_velocity; + velocity_msg.longitudinal_velocity = long_velocity; + velocity_msg.heading_rate = heading_rate; + return velocity_msg; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto aeb_dir = + ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); + + node_options.arguments( + {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + return std::make_shared(node_options); +}; + +std::shared_ptr generatePubSubNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments({"--ros-args"}); + return std::make_shared(node_options); +}; + +PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options) +: Node("test_aeb_pubsub", node_options) +{ + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_imu_ = create_publisher("~/input/imu", qos); + pub_point_cloud_ = create_publisher("~/input/pointcloud", qos); + pub_velocity_ = create_publisher("~/input/velocity", qos); + pub_predicted_traj_ = create_publisher("~/input/predicted_trajectory", qos); + pub_predicted_objects_ = create_publisher("~/input/objects", qos); + pub_autoware_state_ = create_publisher("autoware/state", qos); +} + +TEST_F(TestAEB, checkCollision) +{ + constexpr double longitudinal_velocity = 3.0; + ObjectData object_collision; + object_collision.distance_to_object = 0.5; + object_collision.velocity = 0.1; + ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); + + ObjectData object_no_collision; + object_no_collision.distance_to_object = 10.0; + object_no_collision.velocity = 0.1; + ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision)); +} + +TEST_F(TestAEB, checkImuPathGeneration) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); + + const auto stamp = rclcpp::Time(); + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + const double x_start{0.0}; + const double y_start{0.0}; + + for (size_t i = 0; i < 15; ++i) { + pcl::PointXYZ p1( + x_start + static_cast(i / 100.0), y_start - static_cast(i / 100.0), 0.5); + pcl::PointXYZ p2( + x_start + static_cast((i + 10) / 100.0), y_start - static_cast(i / 100.0), + 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + } + std::vector objects; + aeb_node_->createObjectDataUsingPointCloudClusters( + imu_path, footprint, stamp, objects, obstacle_points_ptr); + ASSERT_FALSE(objects.empty()); +} + +TEST_F(TestAEB, checkIncompleteImuPathGeneration) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double slow_velocity = min_generated_path_length / (2.0 * horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + +TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) +{ + const double velocity = 0.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate); + ASSERT_EQ(imu_path.size(), 1); +} + +TEST_F(TestAEB, checkParamUpdate) +{ + std::vector parameters{rclcpp::Parameter("param")}; + const auto result = aeb_node_->onParameter(parameters); + ASSERT_TRUE(result.successful); +} + +TEST_F(TestAEB, checkEmptyFetchData) +{ + ASSERT_FALSE(aeb_node_->fetchLatestData()); +} + +TEST_F(TestAEB, checkConvertObjectToPolygon) +{ + using autoware_perception_msgs::msg::Shape; + PredictedObject obj_cylinder; + obj_cylinder.shape.type = Shape::CYLINDER; + obj_cylinder.shape.dimensions.x = 1.0; + Pose obj_cylinder_pose; + obj_cylinder_pose.position.x = 1.0; + obj_cylinder_pose.position.y = 1.0; + obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose; + const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder); + ASSERT_FALSE(cylinder_polygon.outer().empty()); + + PredictedObject obj_box; + obj_box.shape.type = Shape::BOUNDING_BOX; + obj_box.shape.dimensions.x = 1.0; + obj_box.shape.dimensions.y = 2.0; + Pose obj_box_pose; + obj_box_pose.position.x = 1.0; + obj_box_pose.position.y = 1.0; + obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose; + const auto box_polygon = utils::convertObjToPolygon(obj_box); + ASSERT_FALSE(box_polygon.outer().empty()); + + geometry_msgs::msg::TransformStamped tf_stamped; + geometry_msgs::msg::Transform transform; + + constexpr double yaw{0.0}; + transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw); + geometry_msgs::msg::Vector3 translation; + translation.x = 1.0; + translation.y = 0.0; + translation.z = 0.0; + transform.translation = translation; + tf_stamped.set__transform(transform); + const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped); + const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose; + Pose expected_pose; + expected_pose.position.x = obj_box_pose.position.x + translation.x; + expected_pose.position.y = obj_box_pose.position.y + translation.y; + expected_pose.position.z = obj_box_pose.position.z + translation.z; + + ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x); + ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y); + ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z); +} + +TEST_F(TestAEB, CollisionDataKeeper) +{ + using namespace std::literals::chrono_literals; + constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0}; + CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock()); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); + ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired()); + + ObjectData obj; + obj.stamp = aeb_node_->now(); + obj.velocity = 0.0; + obj.position.x = 0.0; + rclcpp::sleep_for(100ms); + + ObjectData obj2; + obj2.stamp = aeb_node_->now(); + obj2.velocity = 0.0; + obj2.position.x = 0.1; + rclcpp::sleep_for(100ms); + + constexpr double ego_longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate); + + const auto speed_null = + collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity); + ASSERT_FALSE(speed_null.has_value()); + + const auto median_velocity = + collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity); + ASSERT_TRUE(median_velocity.has_value()); + + // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity + ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2); + rclcpp::sleep_for(1100ms); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); +} + +TEST_F(TestAEB, TestCropPointCloud) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + constexpr size_t n_points{15}; + // Create n_points inside the path and 1 point outside. + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + constexpr double x_start{0.0}; + constexpr double y_start{0.0}; + + for (size_t i = 0; i < n_points; ++i) { + const double offset_1 = static_cast(i / 100.0); + const double offset_2 = static_cast((i + 10) / 100.0); + pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5); + pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5); + obstacle_points_ptr->push_back(p_out); + } + aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_); + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); + aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects); + // Check if the point outside the path was excluded + ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking::test diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp new file mode 100644 index 0000000000000..86edbf73a2a12 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV +// +// 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 TEST_HPP_ +#define TEST_HPP_ + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using std_msgs::msg::Header; + +std::shared_ptr generateNode(); +Header get_header(const char * const frame_id, rclcpp::Time t); +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z); +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate); +class PubSubNode : public rclcpp::Node +{ +public: + explicit PubSubNode(const rclcpp::NodeOptions & node_options); + // publisher + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + // timer + // rclcpp::TimerBase::SharedPtr timer_; + void publishDefaultTopicsNoSpin() + { + const auto header = get_header("base_link", now()); + const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05); + const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0); + + pub_imu_->publish(imu_msg); + pub_velocity_->publish(velocity_msg); + }; +}; + +std::shared_ptr generatePubSubNode(); + +class TestAEB : public ::testing::Test +{ +public: + TestAEB() {} + TestAEB(const TestAEB &) = delete; + TestAEB(TestAEB &&) = delete; + TestAEB & operator=(const TestAEB &) = delete; + TestAEB & operator=(TestAEB &&) = delete; + ~TestAEB() override = default; + + void SetUp() override + { + rclcpp::init(0, nullptr); + pub_sub_node_ = generatePubSubNode(); + aeb_node_ = generateNode(); + } + + void TearDown() override + { + aeb_node_.reset(); + pub_sub_node_.reset(); + rclcpp::shutdown(); + } + + std::shared_ptr pub_sub_node_; + std::shared_ptr aeb_node_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking::test + +#endif // TEST_HPP_ From 48ac496c94f8ff909496eaed7380b77ea4f13bb7 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 23 Aug 2024 09:38:44 +0900 Subject: [PATCH 10/28] feat(autonomous_emergency_braking): enable aeb with only one req path (#8569) * make it so AEB works with only one req path type (imu or MPC) Signed-off-by: Daniel Sanchez * fix missing mpc path return Signed-off-by: Daniel Sanchez * add check Signed-off-by: Daniel Sanchez * modify no path msg Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index e8281a2adae46..512b19c615062 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -348,21 +348,30 @@ bool AEB::fetchLatestData() return missing("object detection method (pointcloud or predicted objects)"); } - const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { + const bool has_imu_path = std::invoke([&]() { + if (!use_imu_path_) return false; + const auto imu_ptr = sub_imu_.takeData(); if (!imu_ptr) { return missing("imu message"); } // imu_ptr is valid onImu(imu_ptr); - } - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } + return (!angular_velocity_ptr_) ? missing("imu") : true; + }); - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); + const bool has_predicted_path = std::invoke([&]() { + if (!use_predicted_trajectory_) { + return false; + } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true; + }); + + if (!has_imu_path && !has_predicted_path) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, + "[AEB] At least one path (IMU or predicted trajectory) is required for operation"); + return false; } autoware_state_ = sub_autoware_state_.takeData(); @@ -482,7 +491,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // 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; + if (!use_imu_path_ || !angular_velocity_ptr_) return false; const double current_w = angular_velocity_ptr_->z; constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; const std::string ns = "ego"; From 07ffb45c5dad6af89b9244afb3be1e6f91fb9fff Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 26 Aug 2024 14:26:00 +0900 Subject: [PATCH 11/28] fix(autonomous_emergency_braking): fix debug marker visual bug (#8611) fix bug by using the collision data keeper Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 5 +--- .../src/node.cpp | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 14 deletions(-) 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 229ea49d89102..fead33f44d1ce 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 @@ -156,10 +156,7 @@ class CollisionDataKeeper * @brief Get the closest object data * @return Object data of the closest object */ - [[nodiscard]] ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } + [[nodiscard]] std::optional get() const { return closest_object_; } /** * @brief Get the previous closest object data diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 512b19c615062..1d2eae17551f7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -393,12 +393,15 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) 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); - if (publish_debug_markers_) { - addCollisionMarker(data, debug_markers); + if (data.has_value()) { + stat.addf("RSS", "%.2f", data.value().rss); + stat.addf("Distance", "%.2f", data.value().distance_to_object); + stat.addf("Object Speed", "%.2f", data.value().velocity); + if (publish_debug_markers_) { + addCollisionMarker(data.value(), debug_markers); + } } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -476,17 +479,19 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return std::make_optional(*closest_object_point_itr); }); + const bool has_collision = (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + // Add debug markers if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, - color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), 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; + return has_collision; }; // step3. make function to check collision with ego path created with sensor data From d98b31b5962dbd006640aabddeeaca99e2a7110d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 26 Aug 2024 17:21:19 +0900 Subject: [PATCH 12/28] docs(autoware_autonomous_emergency_braking): improve AEB module's README (#8612) * docs: improve AEB module's README Signed-off-by: Kyoichi Sugahara * update rss distance length Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../README.md | 8 +- .../image/rss_check.drawio.svg | 183 ++++++------------ 2 files changed, 65 insertions(+), 126 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 1382a24d98b0a..66d4483fbdf84 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -8,8 +8,6 @@ This module has following assumptions. -- It is used when driving at low speeds (about 15 km/h). - - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. - The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles. @@ -25,7 +23,7 @@ The pros and cons of both approaches are: IMU angular velocity: - (+) Usually, it has high accuracy -- (-)Vehicle vibration might introduce noise. +- (-) Vehicle vibration might introduce noise. Steering angle: @@ -186,7 +184,7 @@ The AEB module can also prevent collisions when the ego vehicle is moving backwa When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision. -![backward driving](./image/wrong-mpc.drawio.svg) +![wrong mpc](./image/wrong-mpc.drawio.svg) ## Parameters @@ -217,6 +215,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Limitations +- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly. + - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. - Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg index 2d1716519631d..1d3dd824df5f0 100644 --- a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -1,134 +1,73 @@ - - - - - - - + + + + + + + + + + + + + + + + +
+
+ + Closest point distance +
+ +
+
+ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - Closest Point Distance -
-
-
- - - - - - - - - - - - - - - - - -
-
-
RSS distance
-
-
- - - - + + + + + + +
+
+ RSS distance +
+
+ From 87e56b265d20af354819ff4279fc338218eb0c7f Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 2 Sep 2024 16:55:00 +0900 Subject: [PATCH 13/28] feat(autonomous_emergency_braking): add timekeeper to AEB (#8706) * add timekeeper to AEB Signed-off-by: Daniel Sanchez * add more info to output Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 7 ++- .../src/node.cpp | 54 +++++++++++++------ 2 files changed, 45 insertions(+), 16 deletions(-) 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 fead33f44d1ce..a5289f5688944 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 @@ -15,6 +15,8 @@ #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include "autoware/universe_utils/system/time_keeper.hpp" + #include #include #include @@ -334,9 +336,11 @@ class AEB : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; rclcpp::Publisher::SharedPtr info_marker_publisher_; - + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // timer rclcpp::TimerBase::SharedPtr timer_; + mutable std::shared_ptr time_keeper_{nullptr}; // callback /** @@ -508,6 +512,7 @@ class AEB : public rclcpp::Node // Member variables bool publish_debug_pointcloud_; bool publish_debug_markers_; + bool publish_debug_time_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1d2eae17551f7..b57f8f98333fe 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -181,6 +181,12 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) 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)); + + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); } rcl_interfaces::msg::SetParametersResult AEB::onParameter( @@ -259,6 +265,7 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PointCloud::Ptr pointcloud_ptr(new PointCloud); pcl::fromROSMsg(*input_msg, *pointcloud_ptr); @@ -401,7 +408,6 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) addCollisionMarker(data.value(), debug_markers); } } - addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -416,6 +422,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) bool AEB::checkCollision(MarkerArray & debug_markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using colorTuple = std::tuple; // step1. check data @@ -495,6 +502,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }; // step3. make function to check collision with ego path created with sensor data + time_keeper_->start_track("has_collision_imu_path"); const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { if (!use_imu_path_ || !angular_velocity_ptr_) return false; const double current_w = angular_velocity_ptr_->z; @@ -504,8 +512,10 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return check_collision(ego_path, debug_color, ns, filtered_objects); }; + time_keeper_->end_track("has_collision_imu_path"); // step4. make function to check collision with predicted trajectory from control module + time_keeper_->start_track("has_collision_mpc_path"); const auto has_collision_predicted = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; @@ -519,6 +529,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return check_collision(predicted_path, debug_color, ns, filtered_objects); }; + time_keeper_->end_track("has_collision_mpc_path"); // Data of filtered point cloud pcl::PointCloud::Ptr filtered_objects = @@ -538,10 +549,10 @@ bool AEB::checkCollision(MarkerArray & debug_markers) bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) { - const double & obj_v = closest_object.velocity; - const double & t = t_response_; - + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double rss_dist = std::invoke([&]() { + const double & obj_v = closest_object.velocity; + const double & t = t_response_; const double pre_braking_covered_distance = std::abs(current_v) * t; const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_)); const double ego_stopping_distance = pre_braking_covered_distance + braking_distance; @@ -551,19 +562,19 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object return ego_stopping_distance + obj_braking_distance + 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_.setCollisionData(collision_data); - return true; - } - return false; + if (closest_object.distance_to_object > rss_dist) return false; + + // 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_.setCollisionData(collision_data); + return true; } Path AEB::generateEgoPath(const double curr_v, const double curr_w) { + autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(IMU)", *time_keeper_); Path path; double curr_x = 0.0; double curr_y = 0.0; @@ -612,21 +623,26 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) { + autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(MPC)", *time_keeper_); if (predicted_traj.points.empty()) { return std::nullopt; } - + time_keeper_->start_track("lookUpTransform"); geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, 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"); + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to " + predicted_traj.header.frame_id); return std::nullopt; } + time_keeper_->end_track("lookUpTransform"); // create path + time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); for (size_t i = 0; i < predicted_traj.points.size(); ++i) { @@ -638,12 +654,14 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) break; } } + time_keeper_->end_track("createPath"); return path; } std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( @@ -656,6 +674,7 @@ void AEB::createObjectDataUsingPredictedObjects( const Path & ego_path, const std::vector & ego_polys, std::vector & object_data_vector) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (predicted_objects_ptr_->objects.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; @@ -743,6 +762,7 @@ 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) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the predicted path has valid number of points if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { return; @@ -827,6 +847,7 @@ void AEB::createObjectDataUsingPointCloudClusters( void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); 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 @@ -860,6 +881,7 @@ void AEB::addMarker( const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto path_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), @@ -922,6 +944,7 @@ void AEB::addMarker( void AEB::addVirtualStopWallMarker(MarkerArray & markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto ego_map_pose = std::invoke([this]() -> std::optional { geometry_msgs::msg::TransformStamped tf_current_pose; geometry_msgs::msg::Pose p; @@ -952,6 +975,7 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers) void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto point_marker = autoware::universe_utils::createDefaultMarker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), From 285a0799d1ffd201d3305540e1306c85a29f1ba5 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 3 Sep 2024 18:01:37 +0900 Subject: [PATCH 14/28] feat(autonomous_emergency_braking): increase aeb speed by getting last transform (#8734) set stamp to 0 to get the latest stamp instead of waiting for the stamp Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b57f8f98333fe..c0f261804991b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -250,7 +250,7 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, + "base_link", input_msg->header.frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR_STREAM( @@ -631,7 +631,7 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, + "base_link", predicted_traj.header.frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR_STREAM( @@ -705,7 +705,7 @@ void AEB::createObjectDataUsingPredictedObjects( geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_objects_ptr_->header.frame_id, stamp, + "base_link", predicted_objects_ptr_->header.frame_id, rclcpp::Time(0), 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"); From 8d4f62496f7f4f46c953c8be98c5c068465d226a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 6 Sep 2024 13:02:34 +0900 Subject: [PATCH 15/28] feat(autonomous_emergency_braking): speed up aeb (#8778) * add missing rclcpp::Time(0) Signed-off-by: Daniel Sanchez * refactor to reduce cropping to once per iteration Signed-off-by: Daniel Sanchez * add LookUpTransform to utils Signed-off-by: Daniel Sanchez * separate object creation and clustering Signed-off-by: Daniel Sanchez * error handling of empty pointcloud Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 25 +- .../autonomous_emergency_braking/utils.hpp | 8 +- .../src/node.cpp | 268 +++++++++--------- .../src/utils.cpp | 20 +- .../test/test.cpp | 7 +- 5 files changed, 192 insertions(+), 136 deletions(-) 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 a5289f5688944..abc9fdc23865e 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 @@ -410,6 +410,16 @@ class AEB : public rclcpp::Node */ std::optional generateEgoPath(const Trajectory & predicted_traj); + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @param polygons vector to be filled with the polygons + * @return Vector of polygons representing the path footprint + */ + void generatePathFootprint( + const Path & path, const double extra_width_margin, std::vector & polygons); + /** * @brief Generate the footprint of the path with extra width margin * @param path Ego vehicle path @@ -426,10 +436,19 @@ class AEB : public rclcpp::Node * @param objects Vector to store the created object data * @param obstacle_points_ptr Pointer to the point cloud of obstacles */ - void createObjectDataUsingPointCloudClusters( + void getClosestObjectsOnPath( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, - const pcl::PointCloud::Ptr obstacle_points_ptr); + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); + + /** + * @brief Create object data using point cloud clusters + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + * @param points_belonging_to_cluster_hulls output: pointer to the point cloud of points belonging + * to cluster hulls + */ + void getPointsBelongingToClusterHulls( + const PointCloud::Ptr obstacle_points_ptr, + const PointCloud::Ptr points_belonging_to_cluster_hulls); /** * @brief Create object data using predicted objects diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 2d3ed669e86bb..88910dbfeab18 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -24,6 +24,8 @@ #include #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -50,7 +52,7 @@ using geometry_msgs::msg::TransformStamped; * @param transform_stamped the tf2 transform */ PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped); + const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped); /** * @brief Get the predicted objects polygon as a geometry polygon @@ -82,6 +84,10 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( * @param obj the object */ Polygon2d convertObjToPolygon(const PredictedObject & obj); + +std::optional getTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index c0f261804991b..2e5604c446190 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -45,6 +46,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -247,20 +249,13 @@ void AEB::onTimer() void AEB::onImu(const Imu::ConstSharedPtr input_msg) { // transform imu - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, rclcpp::Time(0), - 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" << input_msg->header.frame_id); - return; - } + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", input_msg->header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return; angular_velocity_ptr_ = std::make_shared(); - tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped.value()); } void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -274,22 +269,15 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) get_logger(), "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, - 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" << input_msg->header.frame_id); - return; - } + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", input_msg->header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return; // transform by using eigen matrix PointCloud2 transformed_points{}; const Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); pcl::fromROSMsg(transformed_points, *pointcloud_ptr); } @@ -442,29 +430,48 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return false; } - auto check_collision = [&]( - const auto & path, const colorTuple & debug_colors, - const std::string & debug_ns, - pcl::PointCloud::Ptr filtered_objects) { + auto merge_expanded_path_polys = [&](const std::vector & paths) { + std::vector merged_expanded_path_polygons; + for (const auto & path : paths) { + generatePathFootprint( + path, expand_width_ + path_footprint_extra_margin_, merged_expanded_path_polygons); + } + return merged_expanded_path_polygons; + }; + + auto get_objects_on_path = [&]( + const auto & path, PointCloud::Ptr points_belonging_to_cluster_hulls, + const colorTuple & debug_colors, const std::string & debug_ns) { // Check which points of the cropped point cloud are on the ego path, and get the closest one const auto ego_polys = generatePathFootprint(path, expand_width_); - auto objects = std::invoke([&]() { - std::vector objects; - // Crop out Pointcloud using an extra wide ego path - if (use_pointcloud_data_) { - const auto expanded_ego_polys = - generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); - cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects, filtered_objects); - } - if (use_predicted_object_data_) { - createObjectDataUsingPredictedObjects(path, ego_polys, objects); - } - return objects; - }); + std::vector objects; + // Crop out Pointcloud using an extra wide ego path + if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + getClosestObjectsOnPath( + path, ego_polys, current_time, points_belonging_to_cluster_hulls, objects); + } + if (use_predicted_object_data_) { + createObjectDataUsingPredictedObjects(path, ego_polys, objects); + } + // Add debug markers + if (publish_debug_markers_) { + const auto [color_r, color_g, color_b, color_a] = debug_colors; + addMarker( + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, + color_g, color_b, color_a, debug_ns, debug_markers); + } + return objects; + }; + + auto check_collision = [&]( + const Path & path, const colorTuple & debug_colors, + const std::string & debug_ns, + PointCloud::Ptr points_belonging_to_cluster_hulls) { + time_keeper_->start_track("has_collision_with_" + debug_ns); + auto objects = + get_objects_on_path(path, points_belonging_to_cluster_hulls, debug_colors, debug_ns); // Get only the closest object and calculate its speed const auto closest_object_point = std::invoke([&]() -> std::optional { const auto closest_object_point_itr = @@ -490,53 +497,61 @@ bool AEB::checkCollision(MarkerArray & debug_markers) ? hasCollision(current_v, closest_object_point.value()) : false; - // Add debug markers - if (publish_debug_markers_) { - const auto [color_r, color_g, color_b, color_a] = debug_colors; - addMarker( - this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, - color_g, color_b, color_a, debug_ns, debug_markers); - } + time_keeper_->end_track("has_collision_with_" + debug_ns); // check collision using rss distance return has_collision; }; // step3. make function to check collision with ego path created with sensor data - time_keeper_->start_track("has_collision_imu_path"); - const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + const auto ego_imu_path = (!use_imu_path_ || !angular_velocity_ptr_) + ? Path{} + : generateEgoPath(current_v, angular_velocity_ptr_->z); + + const auto ego_mpc_path = (!use_predicted_trajectory_ || !predicted_traj_ptr_) + ? std::nullopt + : generateEgoPath(*predicted_traj_ptr_); + + PointCloud::Ptr filtered_objects = pcl::make_shared(); + if (use_pointcloud_data_) { + const std::vector paths = [&]() { + std::vector paths; + if (use_imu_path_) paths.push_back(ego_imu_path); + if (ego_mpc_path.has_value()) { + paths.push_back(ego_mpc_path.value()); + } + return paths; + }(); + + if (paths.empty()) return false; + const std::vector merged_path_polygons = merge_expanded_path_polys(paths); + // Data of filtered point cloud + cropPointCloudWithEgoFootprintPath(merged_path_polygons, filtered_objects); + } + + PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); + getPointsBelongingToClusterHulls(filtered_objects, points_belonging_to_cluster_hulls); + + const auto has_collision_imu_path = + [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { if (!use_imu_path_ || !angular_velocity_ptr_) return false; - const double current_w = angular_velocity_ptr_->z; 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); - - return check_collision(ego_path, debug_color, ns, filtered_objects); + const std::string ns = "imu"; + return check_collision(ego_imu_path, debug_color, ns, points_belonging_to_cluster_hulls); }; - time_keeper_->end_track("has_collision_imu_path"); // step4. make function to check collision with predicted trajectory from control module - time_keeper_->start_track("has_collision_mpc_path"); - 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_; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); - - if (!predicted_path_opt) return false; + const auto has_collision_mpc_path = + [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { + if (!use_predicted_trajectory_ || !ego_mpc_path.has_value()) return false; 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, filtered_objects); + const std::string ns = "mpc"; + return check_collision( + ego_mpc_path.value(), debug_color, ns, points_belonging_to_cluster_hulls); }; - time_keeper_->end_track("has_collision_mpc_path"); - // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects = - pcl::make_shared>(); // evaluate if there is a collision for both paths - const bool has_collision = - has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); + const bool has_collision = has_collision_imu_path(points_belonging_to_cluster_hulls) || + has_collision_mpc_path(points_belonging_to_cluster_hulls); // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { @@ -627,27 +642,17 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) if (predicted_traj.points.empty()) { return std::nullopt; } - time_keeper_->start_track("lookUpTransform"); - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_traj.header.frame_id, rclcpp::Time(0), - 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 " + predicted_traj.header.frame_id); - return std::nullopt; - } - time_keeper_->end_track("lookUpTransform"); - + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", predicted_traj.header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return std::nullopt; // create path time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; - tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { @@ -658,11 +663,21 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) return path; } +void AEB::generatePathFootprint( + const Path & path, const double extra_width_margin, std::vector & polygons) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.push_back( + createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); + } +} + std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); std::vector polygons; + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -702,19 +717,14 @@ void AEB::createObjectDataUsingPredictedObjects( return obj_vel_norm * std::cos(obj_yaw - path_yaw); }; - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_objects_ptr_->header.frame_id, rclcpp::Time(0), - 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; - } - + const auto logger = get_logger(); + const auto transform_stamped_opt = + utils::getTransform("base_link", predicted_objects_ptr_->header.frame_id, tf_buffer_, logger); + if (!transform_stamped_opt.has_value()) return; // Check which objects collide with the ego footprints std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { // get objects in base_link frame + const auto & transform_stamped = transform_stamped_opt.value(); const auto t_predicted_object = utils::transformObjectFrame(predicted_object, transform_stamped); const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose; @@ -758,18 +768,14 @@ void AEB::createObjectDataUsingPredictedObjects( }); } -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) +void AEB::getPointsBelongingToClusterHulls( + const PointCloud::Ptr obstacle_points_ptr, + const PointCloud::Ptr points_belonging_to_cluster_hulls) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { - return; - } - // eliminate noisy points by only considering points belonging to clusters of at least a certain // size + if (obstacle_points_ptr->empty()) return; const std::vector cluster_indices = std::invoke([&]() { std::vector cluster_idx; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); @@ -783,8 +789,6 @@ void AEB::createObjectDataUsingPointCloudClusters( 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); bool cluster_surpasses_threshold_height{false}; @@ -801,12 +805,23 @@ void AEB::createObjectDataUsingPointCloudClusters( hull.setDimension(2); hull.setInputCloud(cluster); std::vector polygons; - PointCloud::Ptr surface_hull(new pcl::PointCloud); + PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); } } +} + +void AEB::getClosestObjectsOnPath( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty() || points_belonging_to_cluster_hulls->empty()) { + return; + } // select points inside the ego footprint path const auto current_p = [&]() { @@ -845,7 +860,7 @@ void AEB::createObjectDataUsingPointCloudClusters( } void AEB::cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) + const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PointCloud::Ptr full_points_ptr(new PointCloud); @@ -863,7 +878,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( hull.setDimension(2); hull.setInputCloud(path_polygon_points); std::vector polygons; - pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); + PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); // Filter out points outside of the path's convex hull pcl::CropHull path_polygon_hull_filter; @@ -946,20 +961,15 @@ void AEB::addVirtualStopWallMarker(MarkerArray & markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto ego_map_pose = std::invoke([this]() -> std::optional { - geometry_msgs::msg::TransformStamped tf_current_pose; + const auto logger = get_logger(); + const auto tf_current_pose = utils::getTransform("map", "base_link", tf_buffer_, logger); + if (!tf_current_pose.has_value()) return std::nullopt; + const auto transform = tf_current_pose.value().transform; geometry_msgs::msg::Pose p; - try { - tf_current_pose = tf_buffer_.lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), "%s", ex.what()); - return std::nullopt; - } - - p.orientation = tf_current_pose.transform.rotation; - p.position.x = tf_current_pose.transform.translation.x; - p.position.y = tf_current_pose.transform.translation.y; - p.position.z = tf_current_pose.transform.translation.z; + p.orientation = transform.rotation; + p.position.x = transform.translation.x; + p.position.y = transform.translation.y; + p.position.z = transform.translation.z; return std::make_optional(p); }); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 0267ee7f6a8d8..f8188d70c8ec3 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -14,6 +14,8 @@ #include +#include + namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; @@ -27,7 +29,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Vector3; PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped) + const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped) { PredictedObject output = input; const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; @@ -151,4 +153,20 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) } return object_polygon; } + +std::optional getTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger) +{ + geometry_msgs::msg::TransformStamped tf_current_pose; + try { + tf_current_pose = tf_buffer.lookupTransform( + target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + logger, "[AEB] Failed to look up transform from " + source_frame + " to " + target_frame); + return std::nullopt; + } + return std::make_optional(tf_current_pose); +} } // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 42054e718d09c..6203e12a78194 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -156,9 +156,12 @@ TEST_F(TestAEB, checkImuPathGeneration) obstacle_points_ptr->push_back(p2); } } + PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); + aeb_node_->getPointsBelongingToClusterHulls( + obstacle_points_ptr, points_belonging_to_cluster_hulls); std::vector objects; - aeb_node_->createObjectDataUsingPointCloudClusters( - imu_path, footprint, stamp, objects, obstacle_points_ptr); + aeb_node_->getClosestObjectsOnPath( + imu_path, footprint, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); } From f35d4a3c0a7aea161c5699e256fe5b3c6dc3a5ea Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Wed, 11 Sep 2024 13:48:36 +0900 Subject: [PATCH 16/28] fix(control): align the parameters with launcher (#8789) align the control parameters Signed-off-by: Zhe Shen --- .../config/autonomous_emergency_braking.param.yaml | 10 +++++----- .../config/control_validator.param.yaml | 2 +- .../config/lane_departure_checker.param.yaml | 8 ++++---- .../param/lateral/mpc.param.yaml | 4 ++-- .../param/longitudinal/pid.param.yaml | 2 +- .../config/vehicle_cmd_gate.param.yaml | 2 +- .../config/obstacle_collision_checker.param.yaml | 4 ++-- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index f13e95c6db8e5..d5c6356c38897 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -2,15 +2,15 @@ ros__parameters: # Ego path calculation use_predicted_trajectory: true - use_imu_path: false + use_imu_path: true use_pointcloud_data: true - use_predicted_object_data: true + use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true 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_horizon: 4.5 mpc_prediction_time_interval: 0.1 # Debug @@ -29,8 +29,8 @@ path_footprint_extra_margin: 4.0 # Point cloud clustering - cluster_tolerance: 0.1 #[m] - cluster_minimum_height: 0.0 + cluster_tolerance: 0.15 #[m] + cluster_minimum_height: 0.1 minimum_cluster_size: 10 maximum_cluster_size: 10000 diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 12709b18b7932..7cce20e56299b 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -6,7 +6,7 @@ # the next trajectory is valid.) diag_error_count_threshold: 0 - display_on_terminal: true # show error msg on terminal + display_on_terminal: false # show error msg on terminal thresholds: max_distance_deviation: 1.0 diff --git a/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml index 2724c28e2536a..84741ae396d07 100644 --- a/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: # Enable feature - will_out_of_lane_checker: true - out_of_lane_checker: true - boundary_departure_checker: false + will_out_of_lane_checker: false + out_of_lane_checker: false + boundary_departure_checker: true # Node update_rate: 10.0 @@ -12,7 +12,7 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - boundary_types_to_detect: [road_border] + boundary_types_to_detect: [curbstone] # Core diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index 9998b6aadf656..37772ac574b1f 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -13,7 +13,7 @@ curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) @@ -46,7 +46,7 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + vehicle_model_steer_tau: 0.27 # steering dynamics time constant (1d approximation) [s] steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5027c94afe7c1..97a1d64ed84f7 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - delay_compensation_time: 0.17 + delay_compensation_time: 0.1 enable_smooth_stop: true enable_overshoot_emergency: true diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 74affea696893..dd99e6d16020d 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -2,7 +2,7 @@ ros__parameters: update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 - use_emergency_handling: false + use_emergency_handling: true check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false enable_cmd_limit_filter: true diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml index 883b4d8259f49..e3c78c90e2ed1 100644 --- a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml +++ b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml @@ -4,8 +4,8 @@ update_rate: 10.0 # Core - delay_time: 0.3 # delay time of vehicle [s] + delay_time: 0.03 # delay time of vehicle [s] footprint_margin: 0.0 # margin for footprint [m] - max_deceleration: 2.0 # max deceleration [m/ss] + max_deceleration: 1.5 # max deceleration [m/ss] resample_interval: 0.3 # interval distance to resample point cloud [m] search_radius: 5.0 # search distance from trajectory to point cloud [m] From 64c787028b6e06b6e41b9cc1340f101d4b248f65 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 12 Sep 2024 18:37:25 +0900 Subject: [PATCH 17/28] feat(autonomous_emergency_braking): add markers showing aeb convex hull polygons for debugging purposes (#8865) * add markers showing aeb convex hull polygons for debugging purposes Signed-off-by: Daniel Sanchez * fix briefs Signed-off-by: Daniel Sanchez * fix typo Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 25 ++++++-- .../autonomous_emergency_braking/utils.hpp | 19 ++++++ .../src/node.cpp | 64 +++++++++++-------- .../src/utils.cpp | 19 ++++++ .../test/test.cpp | 3 +- 5 files changed, 97 insertions(+), 33 deletions(-) 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 abc9fdc23865e..72d3e50822313 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 @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ #include #include #include +#include #include #include namespace autoware::motion::control::autonomous_emergency_braking @@ -72,6 +74,7 @@ using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +using colorTuple = std::tuple; /** * @brief Struct to store object data @@ -448,7 +451,7 @@ class AEB : public rclcpp::Node */ void getPointsBelongingToClusterHulls( const PointCloud::Ptr obstacle_points_ptr, - const PointCloud::Ptr points_belonging_to_cluster_hulls); + const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers); /** * @brief Create object data using predicted objects @@ -475,18 +478,26 @@ class AEB : public rclcpp::Node * @param polygons Polygons representing the ego vehicle footprint * @param objects Vector of object data * @param closest_object Optional data of the closest object - * @param color_r Red color component - * @param color_g Green color component - * @param color_b Blue color component - * @param color_a Alpha (transparency) component + * @param debug_colors Tuple of RGBA colors * @param ns Namespace for the marker * @param debug_markers Marker array for debugging */ void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, 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); + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); + + /** + * @brief Add a marker of convex hulls for debugging + * @param current_time Current time + * @param hulls vector of polygons of the convex hulls + * @param debug_colors Tuple of RGBA colors + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ + void addClusterHullMarkers( + const rclcpp::Time & current_time, const std::vector & hulls, + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); /** * @brief Add a collision marker for debugging diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 88910dbfeab18..443cb6fa6f241 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -26,6 +26,8 @@ #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include @@ -33,6 +35,8 @@ #include #include +#include + #endif namespace autoware::motion::control::autonomous_emergency_braking::utils @@ -85,9 +89,24 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( */ Polygon2d convertObjToPolygon(const PredictedObject & obj); +/** + * @brief Get the transform from source to target frame + * @param target_frame target frame + * @param source_frame source frame + * @param tf_buffer buffer of tf transforms + * @param logger node logger + */ std::optional getTransform( const std::string & target_frame, const std::string & source_frame, const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon2d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 2e5604c446190..8ef71d820c0cc 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -411,7 +410,6 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) bool AEB::checkCollision(MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - using colorTuple = std::tuple; // step1. check data if (!fetchLatestData()) { @@ -457,10 +455,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Add debug markers if (publish_debug_markers_) { - const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, - color_g, color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), + debug_colors, debug_ns, debug_markers); } return objects; }; @@ -529,7 +526,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); - getPointsBelongingToClusterHulls(filtered_objects, points_belonging_to_cluster_hulls); + getPointsBelongingToClusterHulls( + filtered_objects, points_belonging_to_cluster_hulls, debug_markers); const auto has_collision_imu_path = [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { @@ -770,7 +768,7 @@ void AEB::createObjectDataUsingPredictedObjects( void AEB::getPointsBelongingToClusterHulls( const PointCloud::Ptr obstacle_points_ptr, - const PointCloud::Ptr points_belonging_to_cluster_hulls) + const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // eliminate noisy points by only considering points belonging to clusters of at least a certain @@ -789,6 +787,7 @@ void AEB::getPointsBelongingToClusterHulls( ec.extract(cluster_idx); return cluster_idx; }); + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); bool cluster_surpasses_threshold_height{false}; @@ -807,9 +806,19 @@ void AEB::getPointsBelongingToClusterHulls( std::vector polygons; PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); + Polygon2d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); + if (publish_debug_markers_) { + const auto geom_point = autoware::universe_utils::createPoint(p.x, p.y, p.z); + appendPointToPolygon(hull_polygon, geom_point); + } } + hull_polygons.push_back(hull_polygon); + } + if (publish_debug_markers_ && !hull_polygons.empty()) { + constexpr colorTuple debug_color = {255.0 / 256.0, 51.0 / 256.0, 255.0 / 256.0, 0.999}; + addClusterHullMarkers(now(), hull_polygons, debug_color, "hulls", debug_markers); } } @@ -887,23 +896,39 @@ void AEB::cropPointCloudWithEgoFootprintPath( 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; + filtered_objects->header = full_points_ptr->header; +} + +void AEB::addClusterHullMarkers( + const rclcpp::Time & current_time, const std::vector & hulls, + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto [color_r, color_g, color_b, color_a] = debug_colors; + + auto hull_marker = autoware::universe_utils::createDefaultMarker( + "base_link", current_time, ns, 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + utils::fillMarkerFromPolygon(hulls, hull_marker); + debug_markers.markers.push_back(hull_marker); } void AEB::addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, 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) + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto [color_r, color_g, color_b, color_a] = debug_colors; + auto path_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - path_marker.points.resize(path.size()); - for (size_t i = 0; i < path.size(); ++i) { - path_marker.points.at(i) = path.at(i).position; + path_marker.points.reserve(path.size()); + for (const auto & p : path) { + path_marker.points.push_back(p.position); } debug_markers.markers.push_back(path_marker); @@ -911,18 +936,7 @@ void AEB::addMarker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - for (const auto & poly : polygons) { - for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { - const auto & boost_cp = poly.outer().at(dp_idx); - const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = - autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = - autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); - polygon_marker.points.push_back(curr_point); - polygon_marker.points.push_back(next_point); - } - } + utils::fillMarkerFromPolygon(polygons, polygon_marker); debug_markers.markers.push_back(polygon_marker); auto object_data_marker = autoware::universe_utils::createDefaultMarker( diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index f8188d70c8ec3..2bb2f11e28b3e 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include @@ -169,4 +170,22 @@ std::optional getTransform( } return std::make_optional(tf_current_pose); } + +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + } // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 6203e12a78194..11c2fb1ed671d 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -157,8 +157,9 @@ TEST_F(TestAEB, checkImuPathGeneration) } } PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); + MarkerArray debug_markers; aeb_node_->getPointsBelongingToClusterHulls( - obstacle_points_ptr, points_belonging_to_cluster_hulls); + obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); std::vector objects; aeb_node_->getClosestObjectsOnPath( imu_path, footprint, stamp, points_belonging_to_cluster_hulls, objects); From cd97ade3da14621795a8abb4499bc46723f9b9ae Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 13 Sep 2024 15:57:33 +0900 Subject: [PATCH 18/28] docs(autonomous_emergency_braking): make a clearer image for aeb when localization is faulty (#8873) make a clearer image for aeb when localization is faulty Signed-off-by: Daniel Sanchez --- .../image/wrong-mpc.drawio.svg | 415 ++++++++++-------- 1 file changed, 226 insertions(+), 189 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg index a829cb60ec7d6..282d2fff9b9a8 100644 --- a/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg @@ -5,203 +5,240 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="366px" - height="428px" - viewBox="-0.5 -0.5 366 428" - content="<mxfile host="app.diagrams.net" modified="2024-06-13T09:19:00.668Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="zoMk_8KlhUSMgrp7g-GF" version="24.5.4" type="google" scale="5" border="0"> <diagram name="Page-1" id="Er80bHjigqJgTDkSixCH"> <mxGraphModel dx="183" dy="126" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <UserObject label="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" link="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" id="PcQBwLoKRl0nhIK2ECRR-1"> <mxCell style="text;whiteSpace=wrap;html=1;" parent="1" vertex="1"> <mxGeometry x="400" y="215" width="70" height="75" as="geometry" /> </mxCell> </UserObject> <mxCell id="PcQBwLoKRl0nhIK2ECRR-22" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="417" y="216" width="30" height="10" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-5" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1"> <mxGeometry x="407" y="269" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-3" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="412" y="250" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-4" value="&lt;font style=&quot;font-size: 5px;&quot;&gt;IMU&lt;/font&gt;" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="417" y="231" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-7" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;entryX=0.69;entryY=0.13;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="410.9999999999999" y="276" as="sourcePoint" /> <mxPoint x="466.0000000000001" y="230" as="targetPoint" /> <Array as="points"> <mxPoint x="427" y="242" /> </Array> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-13" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" parent="PcQBwLoKRl0nhIK2ECRR-7" connectable="0" vertex="1"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint y="-3" as="offset" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-12" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#0050ef;strokeColor=#001DBC;" parent="1" target="PcQBwLoKRl0nhIK2ECRR-3" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="414.87" y="269" as="sourcePoint" /> <mxPoint x="414.87" y="259" as="targetPoint" /> <Array as="points"> <mxPoint x="414.87" y="269" /> </Array> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-18" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="422.24" y="211" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="425" y="222.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="430.5" y="222.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-25" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="420.24" y="225.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-29" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="415.24" y="280" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-30" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="407" y="276.97" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-2" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=-75;" parent="1" vertex="1"> <mxGeometry x="400" y="274.97" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-31" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="407.24" y="275" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-32" value="" style="endArrow=none;html=1;rounded=0;entryX=0.3;entryY=0.35;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="429" y="259" as="sourcePoint" /> <mxPoint x="421.24" y="252" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-34" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="449.88" y="241" as="sourcePoint" /> <mxPoint x="446" y="234" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-35" value="" style="endArrow=none;html=1;rounded=0;entryX=0.3;entryY=0.35;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="438.26" y="250" as="sourcePoint" /> <mxPoint x="430.5" y="243" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-36" value="" style="endArrow=classic;html=1;rounded=0;strokeColor=default;fillColor=#d5e8d4;endSize=2;startSize=2;" parent="1" edge="1"> <mxGeometry x="0.0036" width="50" height="50" relative="1" as="geometry"> <mxPoint x="460" y="269" as="sourcePoint" /> <mxPoint x="440" y="249" as="targetPoint" /> <mxPoint as="offset" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-37" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC path is wrong&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" parent="1" connectable="0" vertex="1"> <mxGeometry x="456.00037936571215" y="268.99956650554554" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + width="968px" + height="723px" + viewBox="-0.5 -0.5 968 723" + content="<mxfile host="app.diagrams.net" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/128.0.0.0 Safari/537.36" version="24.7.8" scale="9" border="1"> <diagram name="Page-1" id="Y1UgVOXVdMQA1kQdro1d"> <mxGraphModel dx="205" dy="118" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <UserObject label="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" link="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" id="2"> <mxCell style="text;whiteSpace=wrap;html=1;" vertex="1" parent="1"> <mxGeometry x="210" y="220" width="70" height="75" as="geometry" /> </mxCell> </UserObject> <mxCell id="3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#fff2cc;strokeColor=#d6b656;" vertex="1" parent="1"> <mxGeometry x="235" y="221" width="30" height="10" as="geometry" /> </mxCell> <mxCell id="4" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="237.9999999999999" y="279.5" as="sourcePoint" /> <mxPoint x="290" y="250" as="targetPoint" /> <Array as="points"> <mxPoint x="254" y="245.5" /> </Array> </mxGeometry> </mxCell> <mxCell id="5" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" connectable="0" vertex="1" parent="4"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint y="-1" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="249.12" y="229.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="7" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="260" y="225.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="8" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="260" y="230.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="9" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=-75;opacity=50;" vertex="1" parent="1"> <mxGeometry x="224.88" y="279.97" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="10" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="255.12" y="227.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="11" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,iVBORw0KGgoAAAANSUhEUgAAAKwAAABKCAYAAAAmLL9HAAAACXBIWXMAADT9AAA1CgHHClLTAAAJN0lEQVR4nO2dQW7bOBSGfxezLJDMbnbJADWjXdwTVD1B3BNEPcGkJxjNCeKeYJQT1DlBlBPU2QlEgbF33U0MdJ9Z8KmRnkhTylgSJfMDhIC2adHML/LxkXycPD09oQmBEHMAIYAZXUeNvsBz6GwBrOhKMymXTTJP6gg2EOIUQAxgDi9Qz37ZAlgCiDMp17YP7xRsIMQxlFD/2FPhPJ5dfIYS7qPpA0bBBkLMoJR/0k7ZPB4tGwDzTMqV7k2tYAMhIgAL6Lv/vAlPAaxMX+wpEwhRquhMyklfZekTaghnUOMgk4m5BXCVSZnwNyqCJbH+bfiSBYDFribbo8cLtgqZnFd06YT7kYu2JFhSf6rJfAsg8kJ9OV6wZki4CYAL9tYWQFjsxX8KljKtULVZbzIpo7YKeyh4wdoJhEgAXLKXNwBmeWP5S+GNGAMUK/UKIYBj+jsIAiHSvsvQEo8o+1lr98qZlFEgBFAW7QmUyRAD1MKSn/Uflv82k3L+8nK3RyBECCCC9wsPgVsAS90AykQgxBJV8+D3TMr1K0rE7M0tlCCcIhAiCoRYA7iDegq9WN3nAsDfgRBrmiWtQwSlwSIxAOSC5V/klCcgEGIWCLGC8l54v/AwOQHwJRBiSeMlI6S9BXt5DgCTs+l0DuBL4Y0tgFNXBBsIcQXg2vKxeyjvxpqutnkD5Ut8DeA3umycs/TDvgvVAt/p+gFlk36rkafoZzU1Lg9QXiejD59EvUa5F/0wOZtOFyhPvToz0DKMGnPuASRNbKP/WZYQ3m7OJ42STMrU9mGqsxjAO8N3hRbRJij//z9PzqbTlH1hxVnbBzSK1v3Qe6j55rSjcsyguiddWQ6Ze6jZKOtMJwk3QbXFLbmsNPkilCex7l9BNd9Fep9qpSdLJ5BPmZRhh2JdAPhqKMuh8w7AV6qjndD/a4aqGXQC1WKb4FqcTc6mU6cc2oEQMYA/2cvW7mPPZTiGsom53Vkkt5tzv+MYCQt/dz20D1D/H+u4x9Bzfsqk1AqfT7g4JVjqOu7Yyy6JdQNlky1dGZR2BdXLHPoJJqCmaA31axzoc8G+4h/omYSlXRHrFsq2P82kTA5NrIByNdFvPwXwEVU/6TmAtKbLKmIvH0HNZllxRrBkCvAnt5ZRv0cW0LufZi4MRF2B6kJnk56j6j/V5V8BuGEvR3Xu7YRgC8vMitx2KRKaheEutLybW3dVjqFAdRKiKtrLmjNaMUufkEdmJ04IFnrfZq0uYo/wliE3Rw6u+68L1U2IqnlQp5VdQ60zKGIVuiuC5eK86bJVI38fN0fmXqx2qI640E6oTm2kLB3aMvQuWFopxu1G6xO6Z2KWvunK1zsGqK64TRrXyMrHJ6e2DL0LFtWn6qHLgRbZTbx1jbu6/4iIWdpqk2oaBevCJhcFm3Z8f96dPfhBVnOozvgAbO/rqV0Q7ClLpx3fn7cCScf3HxMJS1tH/U1xQbD8R607vv8pS491mrULGtukTfnF/pHWKbmzeohzwAd8Tgu2sMwxxLPNt4HqmWot+2sRXne71mK8CBdaWKdw1ZUVCHFMe53y7UHFAcoJvXZXZ0V/W3RRd16wA6CwxoFvzNNxgRpz+kPFC3YYJGjWvZ5jpINHL1jHIZuVt6wbqNVjEwC/Qq2e2rDPXFDeUeEF6z4RS+fbShLgedkflLeFi5bnHTxesO4TsrQ2fiq9FlvyDh4vWPfh05XGPVCa5Ziji+HgBTswdrmOxuoZKOIF6z4lu9SybI/P3XObdvB4wbpPytKxriUtnEexK+/gcWFq1rObBNXwkyvaA5fbs6bdrEm7Rese38I6Dq0N4FtJTqAiovxLly5I3u0YF6F7wQ6DCM2Cxz1ghD5YwAt2EBQ2+/GWVsctRrx50tuwAyHf7Of48sLW8YIdGCTItOdi9IY3CRiH4Hxviy7qzgXBloIw1In+sWf4YKbr+48JXnd7jzLugmBb3wdkYc3SXrAvp/X9eS4Ids3SYcf35w9M1PH9x0TE0nvfH+eCYFOWDju+P1/9dE7RaDwNMETw2RVd+0W4KNjzLu1Y2qXLF4nEXd1/RMQsvbHtgNbsiLAu1uldsIaIIV1HLoxZ+nKM20vaguqKhyqNa2RtbPP2LliCB3+77LJbpoXP/Onubbv0kKA64l3/pmZs35ClU1sGVwS7xAtijO4Z3qofYcTbpfdBYft549i+1CDxzZVWm9cJwRqOaryoGWN0X2VYohoyMo/bf9pVOYYC1UmK6kDrhurSRszSVpsXcESwAJBJGaPaLS86nki4gj5u/4qOEPXg53GqK+jPg6jTus5QtXmTOvd2RrBExNJ5t9yJaAurorhojwBc04nU0SGaCRQqKT9N/RpVM6DJsUcJe3mLmiagU+d0AYM8WA4Y72KUGYBjOHaw3CPKT8vbHiIIlthxKLLxh7VUjgXKB0d7qnzOpKxjBpgagftMytCQZwZ1dGrO9hU053nWLmpL0Gni95q3rgMh0q58pPSPeGsoy6FzD9W41RFrCL3Nu8HuKN1ciyudYENbAbqAnjo+agdUd3JHwo06KMeKyvKeysPdb4fEFqoO3tMh1daZLDIB7lDdc7aF/aSekKVXk7PpdA7gC/si7bmffUAj0mvLx3J7co1uIni/gXr6XwP4jS4buhG163yn6wdUw/atRp4ZXSHMkWceAES7BE8mxBplc/XD5OnpCYEQ3I79i9xMTkC2TIIWIjp7OucWSqw2b0KM8uB7m0l5nLu1uKP3yiXXDXXLM+jDSnqGwQbAh0xK64F9hqNcl8CzHzZmbx7BwSAMhdOkvT05HG7xfBJ63eWGCap+3hgAJk9Pys1lcOHc0IjdWchcCPHsL3QV7nscq+fhEcreXQFIm46FDC7Nn66zomCP6SbcUHZetEOAO8D7nqBxEYNY8wDOj0BharZwyC3vZi/7PJnEM34KJ+RwsVZcX6W1BORm0DmCLwCsAyG0kfM8npdAQo2h3Fe6E3KuuOvrp0nAviiCWozADV9AqX4J5fdc9T2NOxS8SaCgMUc+7pjDrLEr3SJwrWALX7zECMOOe5xmA2UGaBtC4/JCyjAD8Lmlgnk8nL+gBljGXtvYwhah1eUxzE24x/NSchMzpg2pO6kl2CKBEHMo+yO3RbyAPU3YouynbRS74D+7OeKwBtsqIwAAAABJRU5ErkJggg==;direction=west;imageBackground=default;rotation=-75;" vertex="1" parent="1"> <mxGeometry x="217.26" y="264" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="12" value="" style="endArrow=none;html=1;rounded=0;fillColor=#dae8fc;strokeColor=#6c8ebf;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="231.2399999999999" y="259.5" as="sourcePoint" /> <mxPoint x="283.24" y="230.00000000000003" as="targetPoint" /> <Array as="points"> <mxPoint x="247.24" y="225.50000000000003" /> </Array> </mxGeometry> </mxCell> <mxCell id="13" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;IMU&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" connectable="0" vertex="1" parent="12"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint x="-11" y="6" as="offset" /> </mxGeometry> </mxCell> <mxCell id="14" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.529;entryY=0.366;entryDx=0;entryDy=0;entryPerimeter=0;startSize=5;endSize=1;fillColor=#0050ef;strokeColor=#001DBC;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="234" y="294" as="sourcePoint" /> <mxPoint x="227.49990692359" y="274.9978128903232" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="15" value="&lt;font style=&quot;font-size: 3px;&quot;&gt;Localization error&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="14"> <mxGeometry x="0.0166" relative="1" as="geometry"> <mxPoint x="-32" y="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="16" value="" style="endArrow=classic;html=1;rounded=0;endSize=1;entryX=0.286;entryY=0.8;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="212" y="294" as="sourcePoint" /> <mxPoint x="229.01999999999992" y="284" as="targetPoint" /> </mxGeometry> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " style="background-color: rgb(255, 255, 255);" > - - - - - - - - -
-
-
- - - -
-
-
- + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + +
+
+
+ MPC +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ IMU +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ Localization error +
+
+
+ + + + + + + + + + + + - - - - - - - - - - - - - - - - - - -
-
-
- IMU -
-
-
- - - - - - - - - - - - -
-
-
- MPC -
-
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- MPC path is wrong -
-
-
- - - From 37a2e8ed60d05bb716a2ebf18e529cb04828c4cd Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 20 Sep 2024 23:58:08 +0900 Subject: [PATCH 19/28] feat(autonomous_emergency_braking): make hull markers 3d (#8930) make hull markers 3d Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 3 ++- .../autonomous_emergency_braking/utils.hpp | 9 +++++++++ .../src/node.cpp | 18 ++++++++++++++---- .../src/utils.cpp | 17 +++++++++++++++++ 4 files changed, 42 insertions(+), 5 deletions(-) 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 72d3e50822313..fdd167a7758be 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 @@ -65,6 +65,7 @@ using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -496,7 +497,7 @@ class AEB : public rclcpp::Node * @param debug_markers Marker array for debugging */ void addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); /** diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 443cb6fa6f241..c20e4169bb058 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -42,6 +42,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_universe_utils::Point2d; @@ -107,6 +108,14 @@ std::optional getTransform( */ void fillMarkerFromPolygon( const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon3d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 8ef71d820c0cc..be5eaa1238fcc 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -59,6 +59,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -71,6 +72,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point3d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + point.z() = geom_point.z; + + bg::append(polygon.outer(), point); +} + Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) @@ -580,7 +591,6 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object // 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_.setCollisionData(collision_data); return true; } @@ -787,7 +797,7 @@ void AEB::getPointsBelongingToClusterHulls( ec.extract(cluster_idx); return cluster_idx; }); - std::vector hull_polygons; + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); bool cluster_surpasses_threshold_height{false}; @@ -806,7 +816,7 @@ void AEB::getPointsBelongingToClusterHulls( std::vector polygons; PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); - Polygon2d hull_polygon; + Polygon3d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); if (publish_debug_markers_) { @@ -900,7 +910,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( } void AEB::addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 2bb2f11e28b3e..09dd8a4993855 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -188,4 +188,21 @@ void fillMarkerFromPolygon( } } +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + } // namespace autoware::motion::control::autonomous_emergency_braking::utils From c472c3dda684d39251c317e90a08ec3ecf15b35e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 25 Sep 2024 11:24:19 +0900 Subject: [PATCH 20/28] docs(autonomous_emergency_braking): add missing params to README (#8950) add missing params Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 66d4483fbdf84..28b288093e923 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -56,7 +56,7 @@ We do not activate AEB module if it satisfies the following conditions. - Ego vehicle is not in autonomous driving state -- When the ego vehicle is not moving (Current Velocity is very low) +- When the ego vehicle is not moving (Current Velocity is below a 0.1 m/s threshold) ### 2. Generate a predicted path of the ego vehicle @@ -201,6 +201,10 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | | voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | | voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | | min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | | expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | | longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | From 4d446a25010e4f67da4f545d35f1d151ee4c4313 Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Thu, 26 Sep 2024 07:35:36 +0300 Subject: [PATCH 21/28] feat(autonomous_emergency_braking): calculate the object's velocity in the search area (#8591) * refactor PR Signed-off-by: ismetatabay * WIP Signed-off-by: ismetatabay * change using polygon to lateral offset Signed-off-by: ismetatabay * improve code Signed-off-by: ismetatabay * remove redundant code Signed-off-by: ismetatabay * skip close points in MPC path generation Signed-off-by: ismetatabay * fix empty path points in short parking scenario Signed-off-by: ismetatabay * fix readme conflicts Signed-off-by: ismetatabay --------- Signed-off-by: ismetatabay --- .../README.md | 65 ++--- .../autonomous_emergency_braking.param.yaml | 1 + .../speed_calculation_expansion.drawio.svg | 234 ++++++++++++++++++ .../autonomous_emergency_braking/node.hpp | 8 +- .../package.xml | 1 + .../src/node.cpp | 160 +++++++----- .../test/test.cpp | 5 +- 7 files changed, 385 insertions(+), 89 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 28b288093e923..c3583982dde39 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -112,6 +112,14 @@ Once all target obstacles have been identified, the AEB module chooses the point ### 4. Obstacle velocity estimation +To begin calculating the target point's velocity, the point must enter the speed calculation area, +which is defined by the `speed_calculation_expansion_margin` parameter. +Depending on the operational environment, +this margin can reduce unnecessary autonomous emergency braking +caused by velocity miscalculations during the initial calculation steps. + +![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg) + 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 relative speed using the following equations: $$ @@ -188,34 +196,35 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Parameters -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| publish_debug_markers | [-] | bool | flag to publish debug markers | true | -| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | -| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | -| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | -| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | -| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | -| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | -| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | -| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | -| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | +| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | +| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| speed_calculation_expansion_margin | [m] | double | expansion width of the ego vehicle for the beginning speed calculation | 0.1 | ## Limitations diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index d5c6356c38897..75b54fe547e32 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -27,6 +27,7 @@ # Point cloud cropping expand_width: 0.1 path_footprint_extra_margin: 4.0 + speed_calculation_expansion_margin: 0.5 # Point cloud clustering cluster_tolerance: 0.15 #[m] diff --git a/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg new file mode 100644 index 0000000000000..37728253370e4 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + 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 fdd167a7758be..c6d6f5eef8fff 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 @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ struct ObjectData double velocity{0.0}; double rss{0.0}; double distance_to_object{0.0}; + bool is_target{true}; }; /** @@ -342,6 +344,7 @@ class AEB : public rclcpp::Node rclcpp::Publisher::SharedPtr info_marker_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; mutable std::shared_ptr time_keeper_{nullptr}; @@ -436,12 +439,14 @@ class AEB : public rclcpp::Node * @brief Create object data using point cloud clusters * @param ego_path Ego vehicle path * @param ego_polys Polygons representing the ego vehicle footprint + * @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed + * calculation area * @param stamp Timestamp of the data * @param objects Vector to store the created object data * @param obstacle_points_ptr Pointer to the point cloud of obstacles */ void getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); /** @@ -551,6 +556,7 @@ class AEB : public rclcpp::Node bool use_object_velocity_calculation_; bool check_autoware_state_; double path_footprint_extra_margin_; + double speed_calculation_expansion_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 248d3689072fa..a4adb7087e448 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -41,6 +41,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs visualization_msgs diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index be5eaa1238fcc..847f9fecd10b8 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -139,6 +139,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) 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); + debug_rss_distance_publisher_ = + this->create_publisher("~/debug/rss_distance", 1); } // Diagnostics { @@ -155,6 +157,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); check_autoware_state_ = declare_parameter("check_autoware_state"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); + speed_calculation_expansion_margin_ = + declare_parameter("speed_calculation_expansion_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = declare_parameter("detection_range_max_height_margin"); @@ -215,6 +219,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam( + parameters, "speed_calculation_expansion_margin", speed_calculation_expansion_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); @@ -457,8 +463,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Crop out Pointcloud using an extra wide ego path if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - getClosestObjectsOnPath( - path, ego_polys, current_time, points_belonging_to_cluster_hulls, objects); + getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } if (use_predicted_object_data_) { createObjectDataUsingPredictedObjects(path, ego_polys, objects); @@ -473,39 +478,41 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return objects; }; - auto check_collision = [&]( - const Path & path, const colorTuple & debug_colors, - const std::string & debug_ns, - PointCloud::Ptr points_belonging_to_cluster_hulls) { - time_keeper_->start_track("has_collision_with_" + debug_ns); - auto objects = - get_objects_on_path(path, points_belonging_to_cluster_hulls, debug_colors, debug_ns); - // Get only the closest object and calculate its speed + auto check_collision = [&](const Path & path, std::vector & objects) { + time_keeper_->start_track("has_collision"); const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = + // Attempt to find the closest object + const auto closest_object_itr = std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { + // target objects have priority + if (o1.is_target != o2.is_target) { + return o1.is_target; + } return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects.end()) { - return std::nullopt; - } - const auto closest_object_speed = (use_object_velocity_calculation_) - ? collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v) - : std::make_optional(0.0); - if (!closest_object_speed.has_value()) { - return std::nullopt; + if (closest_object_itr != objects.end()) { + // Calculate speed for the closest object + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_itr, path, current_v) + : std::make_optional(0.0); + + if (closest_object_speed.has_value()) { + closest_object_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_itr); + } } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); + + return std::nullopt; }); - const bool has_collision = (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + const bool has_collision = + (closest_object_point.has_value() && closest_object_point.value().is_target) + ? hasCollision(current_v, closest_object_point.value()) + : false; - time_keeper_->end_track("has_collision_with_" + debug_ns); + time_keeper_->end_track("has_collision"); // check collision using rss distance return has_collision; }; @@ -540,27 +547,46 @@ bool AEB::checkCollision(MarkerArray & debug_markers) getPointsBelongingToClusterHulls( filtered_objects, points_belonging_to_cluster_hulls, debug_markers); - const auto has_collision_imu_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_imu_path_ || !angular_velocity_ptr_) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "imu"; - return check_collision(ego_imu_path, debug_color, ns, points_belonging_to_cluster_hulls); - }; + const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_) + ? std::vector{} + : get_objects_on_path( + ego_imu_path, points_belonging_to_cluster_hulls, + {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu"); + + const auto mpc_path_objects = + (!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value()) + ? std::vector{} + : get_objects_on_path( + ego_mpc_path.value(), points_belonging_to_cluster_hulls, + {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc"); + + // merge object data which comes from the ego (imu) path and predicted path + auto merge_objects = + [&](const std::vector & imu_objects, const std::vector & mpc_objects) { + std::vector merged_objects = imu_objects; + merged_objects.insert(merged_objects.end(), mpc_objects.begin(), mpc_objects.end()); + return merged_objects; + }; + + auto merged_imu_mpc_objects = merge_objects(imu_path_objects, mpc_path_objects); + if (merged_imu_mpc_objects.empty()) return false; - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_mpc_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_predicted_trajectory_ || !ego_mpc_path.has_value()) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "mpc"; - return check_collision( - ego_mpc_path.value(), debug_color, ns, points_belonging_to_cluster_hulls); + // merge path points for the collision checking + auto merge_paths = [&](const std::optional & mpc_path, const Path & imu_path) { + if (!mpc_path.has_value()) { + return imu_path; + } + Path merged_path = imu_path; // Start with imu_path + merged_path.insert( + merged_path.end(), mpc_path.value().begin(), mpc_path.value().end()); // Append mpc_path + return merged_path; }; - // evaluate if there is a collision for both paths - const bool has_collision = has_collision_imu_path(points_belonging_to_cluster_hulls) || - has_collision_mpc_path(points_belonging_to_cluster_hulls); + auto merge_imu_mpc_path = merge_paths(ego_mpc_path, ego_imu_path); + if (merge_imu_mpc_path.empty()) return false; + + // evaluate if there is a collision for merged (imu and mpc) paths + const bool has_collision = check_collision(merge_imu_mpc_path, merged_imu_mpc_objects); // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { @@ -586,6 +612,11 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; }); + tier4_debug_msgs::msg::Float32Stamped rss_distance_msg; + rss_distance_msg.stamp = get_clock()->now(); + rss_distance_msg.data = rss_dist; + debug_rss_distance_publisher_->publish(rss_distance_msg); + if (closest_object.distance_to_object > rss_dist) return false; // collision happens @@ -622,7 +653,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { continue; } path.push_back(current_pose); @@ -636,7 +667,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { continue; } path.push_back(current_pose); @@ -661,6 +692,12 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); + + // skip points that are too close to the last point in the path + if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + continue; + } + path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { @@ -668,7 +705,7 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) } } time_keeper_->end_track("createPath"); - return path; + return (!path.empty()) ? std::make_optional(path) : std::nullopt; } void AEB::generatePathFootprint( @@ -833,12 +870,12 @@ void AEB::getPointsBelongingToClusterHulls( } void AEB::getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || points_belonging_to_cluster_hulls->empty()) { + // check if the predicted path has a valid number of points + if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } @@ -855,6 +892,19 @@ void AEB::getClosestObjectsOnPath( autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; + // calculate the lateral offset between the ego vehicle and the object + const double lateral_offset = + std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); + + if (std::isnan(lateral_offset)) continue; + + // object is outside region of interest + if ( + lateral_offset > + vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) { + continue; + } + // 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; @@ -867,14 +917,8 @@ void AEB::getClosestObjectsOnPath( obj.position = obj_position; obj.velocity = 0.0; obj.distance_to_object = std::abs(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); - break; - } - } + obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_); + objects.push_back(obj); } } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 11c2fb1ed671d..609a7a36b7d67 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -116,6 +116,8 @@ TEST_F(TestAEB, checkCollision) ObjectData object_collision; object_collision.distance_to_object = 0.5; object_collision.velocity = 0.1; + object_collision.position.x = 1.0; + object_collision.position.y = 1.0; ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); ObjectData object_no_collision; @@ -161,8 +163,7 @@ TEST_F(TestAEB, checkImuPathGeneration) aeb_node_->getPointsBelongingToClusterHulls( obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); std::vector objects; - aeb_node_->getClosestObjectsOnPath( - imu_path, footprint, stamp, points_belonging_to_cluster_hulls, objects); + aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); } From 870b2700df74d45233791d23f731edc446d4d826 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 1 Oct 2024 18:19:49 +0900 Subject: [PATCH 22/28] feat(autonomous_emergency_braking): add sanity chackes (#8998) add sanity chackes Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 847f9fecd10b8..4d530dc8a35d7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -461,7 +461,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto ego_polys = generatePathFootprint(path, expand_width_); std::vector objects; // Crop out Pointcloud using an extra wide ego path - if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { + if ( + use_pointcloud_data_ && points_belonging_to_cluster_hulls && + !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } @@ -712,6 +714,9 @@ void AEB::generatePathFootprint( const Path & path, const double extra_width_margin, std::vector & polygons) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return; + } for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -721,8 +726,11 @@ void AEB::generatePathFootprint( std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - std::vector polygons; autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } + std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -735,7 +743,7 @@ void AEB::createObjectDataUsingPredictedObjects( std::vector & object_data_vector) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (predicted_objects_ptr_->objects.empty()) return; + if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; const auto & objects = predicted_objects_ptr_->objects; @@ -926,6 +934,9 @@ void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (ego_polys.empty()) { + return; + } 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 From bead182758bf45ba12d58c124902072d939647d5 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 2 Oct 2024 17:53:50 +0900 Subject: [PATCH 23/28] feat(autonomous_emergency_braking): set max imu path length (#9004) * set a limit to the imu path length Signed-off-by: Daniel Sanchez * fix test and add a new one Signed-off-by: Daniel Sanchez * update readme Signed-off-by: Daniel Sanchez * pre-commit Signed-off-by: Daniel Sanchez * use velocity and time directly to get arc length Signed-off-by: Daniel Sanchez * refactor to reduce repeated code Signed-off-by: Daniel Sanchez * cleaning code Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 3 +- .../autonomous_emergency_braking.param.yaml | 3 +- .../autonomous_emergency_braking/node.hpp | 3 +- .../src/node.cpp | 51 ++++++++++--------- .../test/test.cpp | 22 +++++++- 5 files changed, 53 insertions(+), 29 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index c3583982dde39..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -213,7 +213,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | | minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | | maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | | expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | | longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | | t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 75b54fe547e32..f7548536beaef 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -7,7 +7,8 @@ use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true - min_generated_path_length: 0.5 + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 4.5 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 c6d6f5eef8fff..d14a905ee8df8 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 @@ -562,7 +562,8 @@ class AEB : public rclcpp::Node double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 4d530dc8a35d7..e60d5c88ea62c 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( 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, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -639,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = 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 + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = std::numeric_limits::epsilon(); - const double & dt = imu_prediction_time_interval_; const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -691,12 +689,15 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); // skip points that are too close to the last point in the path - if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { continue; } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 609a7a36b7d67..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -171,7 +171,7 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) { const double dt = aeb_node_->imu_prediction_time_interval_; const double horizon = aeb_node_->imu_prediction_time_horizon_; - const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; const double slow_velocity = min_generated_path_length / (2.0 * horizon); constexpr double yaw_rate = 0.05; const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); @@ -185,6 +185,26 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) ASSERT_TRUE(footprint.size() == imu_path.size() - 1); } +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) { const double velocity = 0.0; From 8c74d5972d3d59412dbd68951dbeb3b8906a7725 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 11 Oct 2024 16:37:01 +0900 Subject: [PATCH 24/28] refactor(autoware_autonomous_emergency_braking): rename info_marker_publisher to virtual_wall_publisher (#9078) Signed-off-by: kyoichi-sugahara --- .../autoware/autonomous_emergency_braking/node.hpp | 2 +- .../autoware_autonomous_emergency_braking/src/node.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) 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 d14a905ee8df8..9e5bb0a9eabba 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 e60d5c88ea62c..31750ed05861f 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -138,7 +138,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 +398,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 +414,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 +423,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) From 755b0166e5593b34863d3a5bbf26b05f9af118a2 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 24 Oct 2024 10:14:24 +0900 Subject: [PATCH 25/28] fix(autonomous_emergency_braking): fix no backward imu path and wrong back distance usage (#9141) * fix no backward imu path and wrong back distance usage Signed-off-by: Daniel Sanchez * use the motion utils isDrivingForward function Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 31750ed05861f..65e49521c5fd9 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 @@ -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; From c033ce1b7de35b2a5276aed3d04ee9221c811677 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 28 Oct 2024 14:41:44 +0900 Subject: [PATCH 26/28] fix namespaces Signed-off-by: Daniel Sanchez --- .../autoware/autonomous_emergency_braking/node.hpp | 4 ++-- .../autoware/autonomous_emergency_braking/utils.hpp | 2 -- .../autoware_autonomous_emergency_braking/src/node.cpp | 8 ++++---- .../autoware_autonomous_emergency_braking/src/utils.cpp | 2 -- 4 files changed, 6 insertions(+), 10 deletions(-) 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 9e5bb0a9eabba..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 @@ -334,9 +334,9 @@ class AEB : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; - autoware_universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + autoware::universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ this, "~/input/objects"}; - autoware_universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ + autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index c20e4169bb058..18f3716b755ee 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -45,8 +45,6 @@ using autoware::universe_utils::Polygon2d; using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 65e49521c5fd9..5d12031f9039b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -755,7 +755,7 @@ void AEB::createObjectDataUsingPredictedObjects( const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); const auto & p = first_point_of_path.position; - return autoware_universe_utils::createPoint(p.x, p.y, p.z); + return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); auto get_object_tangent_velocity = @@ -765,7 +765,7 @@ void AEB::createObjectDataUsingPredictedObjects( predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto obj_yaw = tf2::getYaw(obj_pose.orientation); - const auto obj_idx = autoware_motion_utils::findNearestIndex(ego_path, obj_pose.position); + const auto obj_idx = autoware::motion_utils::findNearestIndex(ego_path, obj_pose.position); const auto path_yaw = (current_ego_speed > 0.0) ? tf2::getYaw(ego_path.at(obj_idx).orientation) : tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI; @@ -796,9 +796,9 @@ void AEB::createObjectDataUsingPredictedObjects( bool collision_points_added{false}; for (const auto & collision_point : collision_points_bg) { const auto obj_position = - autoware_universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + autoware::universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); const double obj_arc_length = - autoware_motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; // If the object is behind the ego, we need to use the backward long offset. The diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 09dd8a4993855..5d9782ada5fbd 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -22,8 +22,6 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_universe_utils::Point2d; -using autoware_universe_utils::Polygon2d; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; From 25aeac1b076ebbfe262036714b341c2867ee064f Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 14 Nov 2024 11:05:42 +0900 Subject: [PATCH 27/28] Revert "fix(control): align the parameters with launcher (#8789)" This reverts commit f35d4a3c0a7aea161c5699e256fe5b3c6dc3a5ea. --- .../config/autonomous_emergency_braking.param.yaml | 10 +++++----- .../config/control_validator.param.yaml | 2 +- .../config/lane_departure_checker.param.yaml | 8 ++++---- .../param/lateral/mpc.param.yaml | 4 ++-- .../param/longitudinal/pid.param.yaml | 2 +- .../config/vehicle_cmd_gate.param.yaml | 2 +- .../config/obstacle_collision_checker.param.yaml | 4 ++-- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index f7548536beaef..01a03b5ae21e8 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -2,16 +2,16 @@ ros__parameters: # Ego path calculation use_predicted_trajectory: true - use_imu_path: true + use_imu_path: false use_pointcloud_data: true - use_predicted_object_data: false + use_predicted_object_data: true use_object_velocity_calculation: true check_autoware_state: true min_generated_imu_path_length: 0.5 max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 - mpc_prediction_time_horizon: 4.5 + mpc_prediction_time_horizon: 1.5 mpc_prediction_time_interval: 0.1 # Debug @@ -31,8 +31,8 @@ speed_calculation_expansion_margin: 0.5 # Point cloud clustering - cluster_tolerance: 0.15 #[m] - cluster_minimum_height: 0.1 + cluster_tolerance: 0.1 #[m] + cluster_minimum_height: 0.0 minimum_cluster_size: 10 maximum_cluster_size: 10000 diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 7cce20e56299b..12709b18b7932 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -6,7 +6,7 @@ # the next trajectory is valid.) diag_error_count_threshold: 0 - display_on_terminal: false # show error msg on terminal + display_on_terminal: true # show error msg on terminal thresholds: max_distance_deviation: 1.0 diff --git a/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml index 84741ae396d07..2724c28e2536a 100644 --- a/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: # Enable feature - will_out_of_lane_checker: false - out_of_lane_checker: false - boundary_departure_checker: true + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false # Node update_rate: 10.0 @@ -12,7 +12,7 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - boundary_types_to_detect: [curbstone] + boundary_types_to_detect: [road_border] # Core diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index 37772ac574b1f..9998b6aadf656 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -13,7 +13,7 @@ curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) @@ -46,7 +46,7 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.27 # steering dynamics time constant (1d approximation) [s] + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 97a1d64ed84f7..5027c94afe7c1 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - delay_compensation_time: 0.1 + delay_compensation_time: 0.17 enable_smooth_stop: true enable_overshoot_emergency: true diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index dd99e6d16020d..74affea696893 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -2,7 +2,7 @@ ros__parameters: update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 - use_emergency_handling: true + use_emergency_handling: false check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false enable_cmd_limit_filter: true diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml index e3c78c90e2ed1..883b4d8259f49 100644 --- a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml +++ b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml @@ -4,8 +4,8 @@ update_rate: 10.0 # Core - delay_time: 0.03 # delay time of vehicle [s] + delay_time: 0.3 # delay time of vehicle [s] footprint_margin: 0.0 # margin for footprint [m] - max_deceleration: 1.5 # max deceleration [m/ss] + max_deceleration: 2.0 # max deceleration [m/ss] resample_interval: 0.3 # interval distance to resample point cloud [m] search_radius: 5.0 # search distance from trajectory to point cloud [m] From 32808d43a1f32c877cea6b4ba7518fd0b5819664 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 6 Nov 2024 18:35:40 +0900 Subject: [PATCH 28/28] fix(autonomous_emergency_braking): solve issue with arc length (#9247) * solve issue with arc length Signed-off-by: Daniel Sanchez * fix problem with points one vehicle apart from path Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 5d12031f9039b..c785ab661060d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -900,11 +900,15 @@ void AEB::getClosestObjectsOnPath( return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); + const auto path_length = autoware::motion_utils::calcArcLength(ego_path); for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); - if (std::isnan(obj_arc_length)) continue; + if ( + std::isnan(obj_arc_length) || + obj_arc_length > path_length + vehicle_info_.max_longitudinal_offset_m) + continue; // calculate the lateral offset between the ego vehicle and the object const double lateral_offset =