From 7ade4b3c777944729f97831a4786e42d18dfd55c Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 17 Jun 2024 18:30:42 +0900 Subject: [PATCH] WIP requires changing path frame to map Signed-off-by: Daniel Sanchez --- .../node.hpp | 7 + .../utils.hpp | 146 ++++++++++++++++++ .../src/node.cpp | 127 +++++++++++++-- 3 files changed, 270 insertions(+), 10 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp 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 2c12cb4314cb5..d9627d1037ccb 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 @@ -63,13 +63,16 @@ using PointCloud = pcl::PointCloud; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; +using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; 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; @@ -285,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); 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..11703469c71cf --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/utils.hpp @@ -0,0 +1,146 @@ +// 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 +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +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 tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +inline 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; +} + +inline 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; +} + +inline 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; +} + +inline 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 + +#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 cb30752e96753..b4eae5f2c7aa1 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,8 @@ #include "autoware_autonomous_emergency_braking/node.hpp" +#include "autoware_autonomous_emergency_braking/utils.hpp" + #include #include #include @@ -21,7 +23,11 @@ #include #include +#include + #include +#include +#include #include #include @@ -46,6 +52,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; @@ -96,7 +103,7 @@ Polygon2d createPolygon( Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; } @@ -378,10 +385,10 @@ 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 check_pointcloud_collision = [&]( + 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_); @@ -390,9 +397,11 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // 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); + // const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + // createObjectDataUsingPointCloudClusters( + // path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + + createObjectDataUsingPredictedObjects(path, ego_polys, objects_from_point_clusters); // Get only the closest object and calculate its speed const auto closest_object_point = std::invoke([&]() -> std::optional { @@ -437,7 +446,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const std::string ns = "ego"; const auto ego_path = generateEgoPath(current_v, current_w); - return check_collision(ego_path, debug_color, ns, filtered_objects); + return check_pointcloud_collision(ego_path, debug_color, ns, filtered_objects); }; // step4. make function to check collision with predicted trajectory from control module @@ -452,7 +461,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const std::string ns = "predicted"; const auto & predicted_path = predicted_path_opt.value(); - return check_collision(predicted_path, debug_color, ns, filtered_objects); + return check_pointcloud_collision(predicted_path, debug_color, ns, filtered_objects); }; // Data of filtered point cloud @@ -578,6 +587,104 @@ 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 tier4_autoware_utils::createPoint(p.x, p.y, p.z); + }(); + + auto get_object_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 = 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); + }; + + // auto convert_polygon_object_to_geometry_polygon = + // []( + // const geometry_msgs::msg::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()); + // object_polygon = tier4_autoware_utils::isClockwise(object_polygon) + // ? object_polygon + // : tier4_autoware_utils::inverseClockwise(object_polygon); + // bg::correct(object_polygon); + // return object_polygon; + // }; + + // Check which objects collide with the ego footprints + std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = convertObjToPolygon(predicted_object); + const double obj_velocity = get_object_velocity(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); + + const bool inter_sec = bg::intersects(obj_poly, ego_poly); + std::cerr << "BEFORE Intersects! " << static_cast(inter_sec) << "\n"; + + std::cerr << "obj_poly.outer().size() " << obj_poly.outer().size() << "\n"; + std::cerr << "ego_poly.outer().size() " << ego_poly.outer().size() << "\n"; + + if (collision_points_bg.empty()) continue; + std::cerr << "Intersects! YEEEEEEEES\n"; + + // Create an object for each intersection point + for (const auto & collision_point : collision_points_bg) { + const auto obj_position = + tier4_autoware_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + const double obj_arc_length = + 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_velocity > 0.0) ? obj_velocity : 0.0; + obj.distance_to_object = std::abs(dist_ego_to_object); + object_data_vector.push_back(obj); + } + } + }); +} + 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)