From e264cf3f702b477de9e60151249d84a74c1a7000 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 24 Apr 2024 17:32:46 +0900 Subject: [PATCH 01/64] add pointcloud to obstacle properties --- .../include/obstacle_cruise_planner/common_structs.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 052e7bb72159..b5a575f0fe6a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -74,10 +74,16 @@ struct Obstacle } } + Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object) + : stamp(arg_stamp), pointcloud(object) + { + } + Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp + PointCloud pointcloud; bool orientation_reliable; Twist twist; bool twist_reliable; From 60224e1334f4613fbc18d1d5bb2ca8e7b0843ff8 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 24 Apr 2024 17:38:09 +0900 Subject: [PATCH 02/64] add tf listener & pointcloud subscriber --- .../include/obstacle_cruise_planner/node.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index c368265ea0f6..81f62d1e4a89 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -27,6 +27,9 @@ #include #include +#include +#include + #include #include #include @@ -143,9 +146,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/odometry"}; tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber pointcloud_sub_{ + this, "~/input/pointcloud"}; tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + // Vehicle Parameters VehicleInfo vehicle_info_; From 028e665505700fa928835b84c6c5a1308916f7b8 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 24 Apr 2024 17:42:19 +0900 Subject: [PATCH 03/64] add parameters for pointcloud obstacle --- .../include/obstacle_cruise_planner/node.hpp | 2 ++ planning/obstacle_cruise_planner/src/node.cpp | 3 +++ 2 files changed, 5 insertions(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 81f62d1e4a89..96b7ead1b120 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -184,6 +184,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onParam(const std::vector & parameters); double decimate_trajectory_step_length; + bool use_pointcloud{false}; + double pointcloud_search_radius; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 43400acb86bd..f1040cfcbe61 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -222,6 +222,9 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara { // behavior determination decimate_trajectory_step_length = node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); + use_pointcloud = node.declare_parameter("behavior_determination.use_pointcloud"); + pointcloud_search_radius = + node.declare_parameter("behavior_determination.pointcloud_search_radius"); obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( From 2f054cdeea2f880cc097a93af53ba57b2e068734 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 24 Apr 2024 17:43:30 +0900 Subject: [PATCH 04/64] add type aliases --- .../include/obstacle_cruise_planner/type_alias.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index d46062dd8f85..6080c09dbfe9 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -27,6 +27,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_planning_msgs/msg/stop_factor.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" @@ -35,6 +36,9 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include + using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; @@ -49,6 +53,7 @@ using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -63,4 +68,6 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using PointCloud = pcl::PointCloud; + #endif // OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ From c41ded8fb1868f019bd0112da62fa834a2864f55 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 24 Apr 2024 17:46:32 +0900 Subject: [PATCH 05/64] convert pointcloud to obstacle --- .../include/obstacle_cruise_planner/node.hpp | 4 +- planning/obstacle_cruise_planner/src/node.cpp | 177 +++++++++++++----- 2 files changed, 131 insertions(+), 50 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 96b7ead1b120..b21c81274fc4 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -58,7 +58,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & traj_points, const vehicle_info_util::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; - std::vector convertToObstacles(const std::vector & traj_points) const; + std::vector convertToObstacles( + const std::vector & traj_points, + const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( const std::vector & traj_points, const std::vector & obstacles); diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f1040cfcbe61..9b1ebbdb1a04 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -23,8 +23,21 @@ #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" +#include + #include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + #include #include @@ -54,6 +67,22 @@ std::optional getObstacleFromUuid( return *itr; } +geometry_msgs::msg::Pose getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + const auto yaw = tier4_autoware_utils::getRPY(base_pose).z; + + geometry_msgs::msg::Pose center_pose; + center_pose.position.x = + base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} + std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) @@ -509,7 +538,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = convertToObstacles(traj_points); + const auto target_obstacles = convertToObstacles(traj_points, msg->header); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = @@ -632,67 +661,117 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( } std::vector ObstacleCruisePlannerNode::convertToObstacles( - const std::vector & traj_points) const + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects_sub_.getData().objects) { - const auto & object_id = - tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); - const auto & current_obstacle_pose = - obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); - - // 1. Check if the obstacle's label is target - const uint8_t label = predicted_object.classification.front().label; - const bool is_target_obstacle = - isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); - if (!is_target_obstacle) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", - object_id.c_str()); - continue; + if (p.use_pointcloud) { + const auto & pointcloud = pointcloud_sub_.getData(); + + // transform pointcloud + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; } - // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = - ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); - const auto ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); - if (!ego_to_obstacle_distance) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", - object_id.c_str()); - continue; + if (transform_stamped) { + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, pointcloud, transformed_points); + PointCloud::Ptr transformed_points_ptr(new PointCloud); + pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + + PointCloud::Ptr obstacle_points_ptr(new PointCloud); + obstacle_points_ptr->header = transformed_points_ptr->header; + + // search obstacle candidate pointcloud to reduce calculation cost + std::vector center_points; + center_points.reserve(traj_points.size()); + for (const auto & traj_point : traj_points) { + center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); + } + for (const auto & point : transformed_points_ptr->points) { + for (const auto & center_point : center_points) { + const double x = center_point.x - point.x; + const double y = center_point.y - point.y; + if (std::hypot(x, y) < p.pointcloud_search_radius) { + obstacle_points_ptr->points.push_back(point); + break; + } + } + } + + const auto target_obstacle = Obstacle(pointcloud.header.stamp, *obstacle_points_ptr); + target_obstacles.push_back(target_obstacle); } + } else { + const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); + + for (const auto & predicted_object : objects_sub_.getData().objects) { + const auto & object_id = + tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); + const auto & current_obstacle_pose = + obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); + + // 1. Check if the obstacle's label is target + const uint8_t label = predicted_object.classification.front().label; + const bool is_target_obstacle = + isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); + if (!is_target_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", + object_id.c_str()); + continue; + } - // 3. Check if rough lateral distance is smaller than the threshold - const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + // 2. Check if the obstacle is in front of the ego. + const size_t ego_idx = + ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); + const auto ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", + object_id.c_str()); + continue; + } - const double min_lat_dist_to_traj_poly = [&]() { - const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); - return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - - obstacle_max_length; - }(); + // 3. Check if rough lateral distance is smaller than the threshold + const double lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + + const double min_lat_dist_to_traj_poly = [&]() { + const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); + return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - + obstacle_max_length; + }(); + + const double max_lat_margin = std::max( + std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), + p.max_lat_margin_for_slow_down); + if (max_lat_margin < min_lat_dist_to_traj_poly) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } - const double max_lat_margin = std::max( - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), - p.max_lat_margin_for_slow_down); - if (max_lat_margin < min_lat_dist_to_traj_poly) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); - continue; + const auto target_obstacle = Obstacle( + obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), + lat_dist_from_obstacle_to_traj); + target_obstacles.push_back(target_obstacle); } - - const auto target_obstacle = Obstacle( - obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), - lat_dist_from_obstacle_to_traj); - target_obstacles.push_back(target_obstacle); } const double calculation_time = stop_watch_.toc(__func__); From a19f5290b8398b7e5b2c73aa2de1387f221dc3be Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 25 Apr 2024 12:06:43 +0900 Subject: [PATCH 06/64] add type alias --- .../include/obstacle_cruise_planner/type_alias.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index 6080c09dbfe9..f55b3546508e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -65,6 +65,7 @@ using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; +using tier4_autoware_utils::MultiPoint2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; From 29f9284253fc559810adb41a0b86fe67e1b4b335 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 25 Apr 2024 12:11:14 +0900 Subject: [PATCH 07/64] add polygon conversion for pointcloud obstacle --- .../common_structs.hpp | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index b5a575f0fe6a..c6b31907e5a8 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -25,6 +25,8 @@ #include +#include + #include #include #include @@ -66,7 +68,8 @@ struct Obstacle uuid(tier4_autoware_utils::toHexString(object.object_id)), shape(object.shape), ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + pointcloud_repr(false) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -75,15 +78,27 @@ struct Obstacle } Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object) - : stamp(arg_stamp), pointcloud(object) + : stamp(arg_stamp), uuid(""), pointcloud(object), pointcloud_repr(true) { } - Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } + Polygon2d toPolygon() const + { + if (pointcloud_repr) { + MultiPoint2d points; + for (const auto & point : pointcloud) { + bg::append(points, Point2d(point.x, point.y)); + } + Polygon2d polygon; + bg::convex_hull(points, polygon); + return polygon; + } else { + return tier4_autoware_utils::toPolygon2d(pose, shape); + } + } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp - PointCloud pointcloud; bool orientation_reliable; Twist twist; bool twist_reliable; @@ -93,6 +108,8 @@ struct Obstacle std::vector predicted_paths; double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; + PointCloud pointcloud; + bool pointcloud_repr; }; struct TargetObstacleInterface From ce95e63709478220d18562b77b4c31b7ccb62070 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 25 Apr 2024 12:13:45 +0900 Subject: [PATCH 08/64] initialize twist & pose of pointcloud obstacle --- .../include/obstacle_cruise_planner/common_structs.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index c6b31907e5a8..868c2446f9da 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -27,6 +27,8 @@ #include +#include + #include #include #include @@ -80,6 +82,13 @@ struct Obstacle Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object) : stamp(arg_stamp), uuid(""), pointcloud(object), pointcloud_repr(true) { + twist.linear.x = twist.linear.y = twist.angular.z = 0.0; + + Eigen::Vector4d centroid; + pcl::compute3DCentroid(pointcloud, centroid); + pose.position.x = centroid.x(); + pose.position.y = centroid.y(); + pose.position.z = centroid.z(); } Polygon2d toPolygon() const From 30ccefed751007bfd4766796a1bc92ff08ea98b7 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 25 Apr 2024 12:19:21 +0900 Subject: [PATCH 09/64] overload to handle both obstacle & predicted path --- .../src/polygon_utils.cpp | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index fa5a96b934f7..5ebf124e6ae5 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -48,10 +48,9 @@ PointWithStamp calcNearestCollisionPoint( // calculation. std::optional>> getCollisionIndex( const std::vector & traj_points, const std::vector & traj_polygons, - const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, - const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) + const Polygon2d & object_polygon, const geometry_msgs::msg::Pose & object_pose, + const rclcpp::Time & object_time, const double max_lat_dist = std::numeric_limits::max()) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); @@ -60,7 +59,7 @@ std::optional>> getCollisionIndex( } std::vector collision_polygons; - boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); + boost::geometry::intersection(traj_polygons.at(i), object_polygon, collision_polygons); std::vector collision_geom_points; bool has_collision = false; @@ -88,6 +87,24 @@ std::optional>> getCollisionIndex( return std::nullopt; } + +std::optional>> getCollisionIndex( + const std::vector & traj_points, const std::vector & traj_polygons, + const Obstacle & obstacle, const double max_lat_dist = std::numeric_limits::max()) +{ + return getCollisionIndex( + traj_points, traj_polygons, obstacle.toPolygon(), obstacle.pose, obstacle.stamp); +} + +std::optional>> getCollisionIndex( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, + const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) +{ + return getCollisionIndex( + traj_points, traj_polygons, tier4_autoware_utils::toPolygon2d(object_pose, object_shape), + object_pose, object_time); +} } // namespace namespace polygon_utils @@ -97,8 +114,7 @@ std::optional> getCollisionPoint( const Obstacle & obstacle, const bool is_driving_forward, const vehicle_info_util::VehicleInfo & vehicle_info) { - const auto collision_info = - getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape); + const auto collision_info = getCollisionIndex(traj_points, traj_polygons, obstacle); if (!collision_info) { return std::nullopt; } From 23c1fff47b54c3e7b010d86b2d81a9378eafe7f5 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 25 Apr 2024 12:21:55 +0900 Subject: [PATCH 10/64] implement ego behavior determination against pointcloud obstacles --- planning/obstacle_cruise_planner/src/node.cpp | 195 ++++++++++-------- 1 file changed, 108 insertions(+), 87 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 9b1ebbdb1a04..f8e690eb10ce 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -123,6 +123,10 @@ double calcDiffAngleAgainstTrajectory( std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { + if (obstacle.pointcloud_repr) { + return std::make_pair(0.0, 0.0); + } + const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); @@ -842,6 +846,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector cruise_obstacles; std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); + const auto & p = behavior_determination_param_; for (const auto & obstacle : obstacles) { const auto obstacle_poly = obstacle.toPolygon(); @@ -852,12 +857,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); } - // Filter obstacles for cruise, stop and slow down - const auto cruise_obstacle = - createCruiseObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); - if (cruise_obstacle) { - cruise_obstacles.push_back(*cruise_obstacle); - continue; + if (!p.use_pointcloud) { + // Filter obstacles for cruise, stop and slow down + const auto cruise_obstacle = createCruiseObstacle( + decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + if (cruise_obstacle) { + cruise_obstacles.push_back(*cruise_obstacle); + continue; + } } const auto stop_obstacle = createStopObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); @@ -872,7 +879,6 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( continue; } } - const auto & p = behavior_determination_param_; if (p.enable_yield) { const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points); if (yield_obstacles) { @@ -891,8 +897,10 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } slow_down_condition_counter_.removeCounterUnlessUpdated(); - // Check target obstacles' consistency - checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); + if (!p.use_pointcloud) { + // Check target obstacles' consistency + checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); + } // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -1240,59 +1248,61 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: consider all target obstacles when driving backward - if (!isStopObstacle(obstacle.classification.label)) { + if (!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) { return std::nullopt; } if (p.max_lat_margin_for_stop < precise_lat_dist) { return std::nullopt; } - // check obstacle velocity - // NOTE: If precise_lat_dist is 0, always plan stop - constexpr double epsilon = 1e-6; - if (epsilon < precise_lat_dist) { - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { - return std::nullopt; + if (!p.use_pointcloud) { + // check obstacle velocity + // NOTE: If precise_lat_dist is 0, always plan stop + constexpr double epsilon = 1e-6; + if (epsilon < precise_lat_dist) { + const auto [obstacle_tangent_vel, obstacle_normal_vel] = + projectObstacleVelocityToTrajectory(traj_points, obstacle); + if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { + return std::nullopt; + } } - } - // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, - // and the collision between ego and obstacles are within the margin threshold. - const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - if (is_obstacle_crossing && has_high_speed) { - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( - obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); - - std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); - if (collision_points.empty()) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " - "predicted path " - "and the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; - } + // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, + // and the collision between ego and obstacles are within the margin threshold. + const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); + const double has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + if (is_obstacle_crossing && has_high_speed) { + // Get highest confidence predicted path + const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon); + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } - const double collision_time_margin = - calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_); - if (p.collision_time_margin < collision_time_margin) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; + const double collision_time_margin = + calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } } } @@ -1318,49 +1328,60 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; - slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); - const bool is_prev_obstacle_slow_down = - getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + if (!p.use_pointcloud) { + slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); + } - if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { + if ( + !enable_slow_down_planning_ || + (!p.use_pointcloud && !isSlowDownObstacle(obstacle.classification.label))) { return std::nullopt; } - // check lateral distance considering hysteresis - const bool is_lat_dist_low = isLowerConsideringHysteresis( - precise_lat_dist, is_prev_obstacle_slow_down, - p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, - p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); - - const bool is_slow_down_required = [&]() { - if (is_prev_obstacle_slow_down) { - // check if exiting slow down - if (!is_lat_dist_low) { - const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); - if (count <= -p.successive_num_to_exit_slow_down_condition) { - slow_down_condition_counter_.reset(obstacle.uuid); - return false; - } - } - return true; + if (p.use_pointcloud) { + if (p.max_lat_margin_for_slow_down < precise_lat_dist) { + return std::nullopt; } - // check if entering slow down - if (is_lat_dist_low) { - const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); - if (p.successive_num_to_entry_slow_down_condition <= count) { - slow_down_condition_counter_.reset(obstacle.uuid); + } else { + const bool is_prev_obstacle_slow_down = + getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = isLowerConsideringHysteresis( + precise_lat_dist, is_prev_obstacle_slow_down, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, + p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obstacle.uuid); + return false; + } + } return true; } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obstacle.uuid); + return true; + } + } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", + object_id.c_str(), precise_lat_dist); + return std::nullopt; } - return false; - }(); - if (!is_slow_down_required) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), - precise_lat_dist); - return std::nullopt; } const auto obstacle_poly = obstacle.toPolygon(); From 89247712cd0ae26a9ca236a06ae94c351f35a0ea Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 11:59:08 +0900 Subject: [PATCH 11/64] generate obstacle from point --- .../common_structs.hpp | 41 +++++++------------ 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 868c2446f9da..e784a0efb683 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -70,8 +70,7 @@ struct Obstacle uuid(tier4_autoware_utils::toHexString(object.object_id)), shape(object.shape), ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), - pointcloud_repr(false) + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -79,32 +78,22 @@ struct Obstacle } } - Obstacle(const rclcpp::Time & arg_stamp, const PointCloud & object) - : stamp(arg_stamp), uuid(""), pointcloud(object), pointcloud_repr(true) + Obstacle( + const rclcpp::Time & arg_stamp, const geometry_msgs::msg::Point & position, + const double diameter, const double ego_to_obstacle_distance, + const double lat_dist_from_obstacle_to_traj) + : stamp(arg_stamp), + orientation_reliable(false), + twist_reliable(false), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) { - twist.linear.x = twist.linear.y = twist.angular.z = 0.0; - - Eigen::Vector4d centroid; - pcl::compute3DCentroid(pointcloud, centroid); - pose.position.x = centroid.x(); - pose.position.y = centroid.y(); - pose.position.z = centroid.z(); + pose.position = position; + shape.type = Shape::CYLINDER; + shape.dimensions.x = diameter; } - Polygon2d toPolygon() const - { - if (pointcloud_repr) { - MultiPoint2d points; - for (const auto & point : pointcloud) { - bg::append(points, Point2d(point.x, point.y)); - } - Polygon2d polygon; - bg::convex_hull(points, polygon); - return polygon; - } else { - return tier4_autoware_utils::toPolygon2d(pose, shape); - } - } + Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. geometry_msgs::msg::Pose pose; // interpolated with the current stamp @@ -117,8 +106,6 @@ struct Obstacle std::vector predicted_paths; double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; - PointCloud pointcloud; - bool pointcloud_repr; }; struct TargetObstacleInterface From 0d99190c4636c6c5a03047aca3197a3ee4154ac2 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 12:00:59 +0900 Subject: [PATCH 12/64] revert getCollisionIndex() --- .../src/polygon_utils.cpp | 29 +++++-------------- 1 file changed, 7 insertions(+), 22 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 5ebf124e6ae5..343809924e07 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -48,8 +48,8 @@ PointWithStamp calcNearestCollisionPoint( // calculation. std::optional>> getCollisionIndex( const std::vector & traj_points, const std::vector & traj_polygons, - const Polygon2d & object_polygon, const geometry_msgs::msg::Pose & object_pose, - const rclcpp::Time & object_time, const double max_lat_dist = std::numeric_limits::max()) + const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, + const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) { for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = @@ -59,7 +59,9 @@ std::optional>> getCollisionIndex( } std::vector collision_polygons; - boost::geometry::intersection(traj_polygons.at(i), object_polygon, collision_polygons); + boost::geometry::intersection( + traj_polygons.at(i), tier4_autoware_utils::toPolygon2d(object_pose, object_shape), + collision_polygons); std::vector collision_geom_points; bool has_collision = false; @@ -87,24 +89,6 @@ std::optional>> getCollisionIndex( return std::nullopt; } - -std::optional>> getCollisionIndex( - const std::vector & traj_points, const std::vector & traj_polygons, - const Obstacle & obstacle, const double max_lat_dist = std::numeric_limits::max()) -{ - return getCollisionIndex( - traj_points, traj_polygons, obstacle.toPolygon(), obstacle.pose, obstacle.stamp); -} - -std::optional>> getCollisionIndex( - const std::vector & traj_points, const std::vector & traj_polygons, - const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, - const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) -{ - return getCollisionIndex( - traj_points, traj_polygons, tier4_autoware_utils::toPolygon2d(object_pose, object_shape), - object_pose, object_time); -} } // namespace namespace polygon_utils @@ -114,7 +98,8 @@ std::optional> getCollisionPoint( const Obstacle & obstacle, const bool is_driving_forward, const vehicle_info_util::VehicleInfo & vehicle_info) { - const auto collision_info = getCollisionIndex(traj_points, traj_polygons, obstacle); + const auto collision_info = + getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape); if (!collision_info) { return std::nullopt; } From 035ad9974552eaeda736ae878b03ebcf183cf00a Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 12:01:53 +0900 Subject: [PATCH 13/64] generate obstacle from each point in cloud --- planning/obstacle_cruise_planner/src/node.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f8e690eb10ce..b0ef588a5724 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -709,15 +709,20 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( for (const auto & center_point : center_points) { const double x = center_point.x - point.x; const double y = center_point.y - point.y; - if (std::hypot(x, y) < p.pointcloud_search_radius) { - obstacle_points_ptr->points.push_back(point); + const double ego_to_obstacle_distance = std::hypot(x, y); + if (ego_to_obstacle_distance < p.pointcloud_search_radius) { + geometry_msgs::msg::Point obstacle_position; + obstacle_position.x = point.x; + obstacle_position.y = point.y; + const double lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, obstacle_position); + target_obstacles.emplace_back( + pointcloud.header.stamp, obstacle_position, 1e-3, ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj); break; } } } - - const auto target_obstacle = Obstacle(pointcloud.header.stamp, *obstacle_points_ptr); - target_obstacles.push_back(target_obstacle); } } else { const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); From b724add87e3c654c2003a871d6a0156e42aea258 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 12:03:16 +0900 Subject: [PATCH 14/64] set pointcloud obstacle velocity to 0 --- planning/obstacle_cruise_planner/src/node.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index b0ef588a5724..5e552e2e3299 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -123,10 +123,6 @@ double calcDiffAngleAgainstTrajectory( std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { - if (obstacle.pointcloud_repr) { - return std::make_pair(0.0, 0.0); - } - const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); @@ -1321,7 +1317,12 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + const auto [tangent_vel, normal_vel] = [&]() -> std::pair { + if (p.use_pointcloud) { + return {0., 0.}; + } + return projectObstacleVelocityToTrajectory(traj_points, obstacle); + }(); return StopObstacle{ obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, collision_point->first, collision_point->second}; From 1a3151cd6d295b5177d8403cd54bbfe698e30ba1 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 15:27:35 +0900 Subject: [PATCH 15/64] use tf buffer & listener with pointers --- .../include/obstacle_cruise_planner/node.hpp | 4 ++-- planning/obstacle_cruise_planner/src/node.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index b21c81274fc4..fef3b47f78aa 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -153,8 +153,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; - tf2_ros::Buffer tf_buffer_{get_clock()}; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 5e552e2e3299..1f3bcb6d70c6 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -411,6 +411,10 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_slow_down_planning_info_pub_ = create_publisher("~/debug/slow_down_planning_info", 1); + // tf listener + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + const auto longitudinal_info = LongitudinalInfo(*this); ego_nearest_param_ = EgoNearestParam(*this); @@ -674,7 +678,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( // transform pointcloud std::optional transform_stamped{}; try { - transform_stamped = tf_buffer_.lookupTransform( + transform_stamped = tf_buffer_->lookupTransform( traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { From 7460587c98b1d8fc43a68cc2602c37f53b578043 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 15:27:55 +0900 Subject: [PATCH 16/64] update latest pointcloud data --- planning/obstacle_cruise_planner/src/node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 1f3bcb6d70c6..1cae59949464 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -520,9 +520,10 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { + const auto & p = behavior_determination_param_; if ( - !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() || - !acc_sub_.updateLatestData()) { + !ego_odom_sub_.updateLatestData() || (!p.use_pointcloud && !objects_sub_.updateLatestData()) || + (p.use_pointcloud && !pointcloud_sub_.updateLatestData()) || !acc_sub_.updateLatestData()) { return; } From 8df16ff5781a307998a95b148b5a5dfcf8278788 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 15:58:24 +0900 Subject: [PATCH 17/64] add topic remap --- .../lane_driving/motion_planning/motion_planning.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 18de04fd9e31..fcfb1e1dc46d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -125,6 +125,7 @@ + From fede0ff5b158a7efe8017d849d4870f52d227c1b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 1 May 2024 17:01:46 +0900 Subject: [PATCH 18/64] remove unnecessary includes --- .../include/obstacle_cruise_planner/common_structs.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index e784a0efb683..e4f18399df0d 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -25,10 +25,6 @@ #include -#include - -#include - #include #include #include From 25731b3a48309c3ba5a0b2985565578c5b87fae4 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 2 May 2024 11:02:59 +0900 Subject: [PATCH 19/64] set slow down obstacle velocity to 0 --- planning/obstacle_cruise_planner/src/node.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 1cae59949464..124ea5988ca4 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1462,7 +1462,12 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + const auto [tangent_vel, normal_vel] = [&]() -> std::pair { + if (p.use_pointcloud) { + return {0., 0.}; + } + return projectObstacleVelocityToTrajectory(traj_points, obstacle); + }(); return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, normal_vel, precise_lat_dist, front_collision_point, back_collision_point}; From c57545dfd3b22b81940364d19456451e154aea00 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 2 May 2024 13:29:19 +0900 Subject: [PATCH 20/64] add flag to consider pointcloud obstacle for stopping & slowing down --- .../include/obstacle_cruise_planner/node.hpp | 2 ++ planning/obstacle_cruise_planner/src/node.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index fef3b47f78aa..4f0e70b93c99 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -125,6 +125,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 124ea5988ca4..fe7d7a0f60f7 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -462,6 +462,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & outside_cruise_obstacle_types_ = getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); + use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = declare_parameter("common.slow_down_obstacle_type.pointcloud"); } // set parameter callback @@ -1254,7 +1256,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: consider all target obstacles when driving backward - if (!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) { + if ((!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) || (p.use_pointcloud && !use_pointcloud_for_stop_)) { return std::nullopt; } if (p.max_lat_margin_for_stop < precise_lat_dist) { @@ -1346,7 +1348,8 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl if ( !enable_slow_down_planning_ || - (!p.use_pointcloud && !isSlowDownObstacle(obstacle.classification.label))) { + (!p.use_pointcloud && !isSlowDownObstacle(obstacle.classification.label)) || + (p.use_pointcloud && !use_pointcloud_for_slow_down_)) { return std::nullopt; } From 3b6a30c5d2f4074adf9670a4f7252f3736ad24f2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 2 May 2024 04:31:18 +0000 Subject: [PATCH 21/64] style(pre-commit): autofix --- planning/obstacle_cruise_planner/src/node.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index fe7d7a0f60f7..1f38c6611a83 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -463,7 +463,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); - use_pointcloud_for_slow_down_ = declare_parameter("common.slow_down_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = + declare_parameter("common.slow_down_obstacle_type.pointcloud"); } // set parameter callback @@ -1256,7 +1257,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: consider all target obstacles when driving backward - if ((!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) || (p.use_pointcloud && !use_pointcloud_for_stop_)) { + if ( + (!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) || + (p.use_pointcloud && !use_pointcloud_for_stop_)) { return std::nullopt; } if (p.max_lat_margin_for_stop < precise_lat_dist) { From 844393a37417c91ef27bd0c3f5d9af9eda421b8c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 2 May 2024 15:25:31 +0900 Subject: [PATCH 22/64] downsample pointcloud using voxel grid --- .../include/obstacle_cruise_planner/node.hpp | 3 +++ planning/obstacle_cruise_planner/src/node.cpp | 18 ++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 4f0e70b93c99..46d2f58b7eb9 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -190,6 +190,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double decimate_trajectory_step_length; bool use_pointcloud{false}; double pointcloud_search_radius; + double pointcloud_voxel_grid_x; + double pointcloud_voxel_grid_y; + double pointcloud_voxel_grid_z; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 1f38c6611a83..7eaa98d1771a 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -27,6 +27,7 @@ #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -254,6 +255,12 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara use_pointcloud = node.declare_parameter("behavior_determination.use_pointcloud"); pointcloud_search_radius = node.declare_parameter("behavior_determination.pointcloud_search_radius"); + pointcloud_voxel_grid_x = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_x"); + pointcloud_voxel_grid_y = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); + pointcloud_voxel_grid_z = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( @@ -700,8 +707,15 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( PointCloud::Ptr transformed_points_ptr(new PointCloud); pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(transformed_points_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + PointCloud::Ptr obstacle_points_ptr(new PointCloud); - obstacle_points_ptr->header = transformed_points_ptr->header; + obstacle_points_ptr->header = filtered_points_ptr->header; // search obstacle candidate pointcloud to reduce calculation cost std::vector center_points; @@ -709,7 +723,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( for (const auto & traj_point : traj_points) { center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); } - for (const auto & point : transformed_points_ptr->points) { + for (const auto & point : filtered_points_ptr->points) { for (const auto & center_point : center_points) { const double x = center_point.x - point.x; const double y = center_point.y - point.y; From 4c7a339ce19dbd5fccff6365bcbf67e164769967 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 8 May 2024 11:47:58 +0900 Subject: [PATCH 23/64] change shape type of pointcloud obstacle to polygon --- .../obstacle_cruise_planner/common_structs.hpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index e4f18399df0d..e98b0436c17d 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -76,7 +76,7 @@ struct Obstacle Obstacle( const rclcpp::Time & arg_stamp, const geometry_msgs::msg::Point & position, - const double diameter, const double ego_to_obstacle_distance, + const geometry_msgs::msg::Polygon & footprint, const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), orientation_reliable(false), @@ -85,8 +85,13 @@ struct Obstacle lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) { pose.position = position; - shape.type = Shape::CYLINDER; - shape.dimensions.x = diameter; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + pose.orientation.w = 1; + + shape.type = Shape::POLYGON; + shape.footprint = footprint; } Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } From fb00bed8ab45ef9819f9699614b980962cc903bb Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 8 May 2024 11:51:50 +0900 Subject: [PATCH 24/64] convert pointcloud to obstacle by clustering --- planning/obstacle_cruise_planner/src/node.cpp | 77 +++++++++++++------ 1 file changed, 55 insertions(+), 22 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 7eaa98d1771a..7c0b0b589e5f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -27,7 +27,10 @@ #include +#include #include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -229,6 +232,18 @@ geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & poin return geom_point; } +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon geom_poly; + geometry_msgs::msg::Point32 point; + for (const auto & p : polygon.outer()) { + point.x = p.x(); + point.y = p.y(); + geom_poly.points.push_back(point); + } + return geom_poly; +} + bool isLowerConsideringHysteresis( const double current_val, const bool was_low, const double high_val, const double low_val) { @@ -699,7 +714,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( transform_stamped = std::nullopt; } - if (transform_stamped) { + if (transform_stamped && !pointcloud.data.empty()) { PointCloud2 transformed_points{}; const Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); @@ -714,30 +729,48 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); filter.filter(*filtered_points_ptr); - PointCloud::Ptr obstacle_points_ptr(new PointCloud); - obstacle_points_ptr->header = filtered_points_ptr->header; - - // search obstacle candidate pointcloud to reduce calculation cost - std::vector center_points; - center_points.reserve(traj_points.size()); + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(1); + ec.setMinClusterSize(1); + ec.setMaxClusterSize(100000); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + std::vector center_points(traj_points.size()); for (const auto & traj_point : traj_points) { center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); } - for (const auto & point : filtered_points_ptr->points) { - for (const auto & center_point : center_points) { - const double x = center_point.x - point.x; - const double y = center_point.y - point.y; - const double ego_to_obstacle_distance = std::hypot(x, y); - if (ego_to_obstacle_distance < p.pointcloud_search_radius) { - geometry_msgs::msg::Point obstacle_position; - obstacle_position.x = point.x; - obstacle_position.y = point.y; - const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, obstacle_position); - target_obstacles.emplace_back( - pointcloud.header.stamp, obstacle_position, 1e-3, ego_to_obstacle_distance, - lat_dist_from_obstacle_to_traj); - break; + + // search obstacle candidate pointcloud to reduce calculation cost + for (const auto & cluster_indices : clusters) { + Eigen::Vector4d centroid; + if (pcl::compute3DCentroid(*filtered_points_ptr, cluster_indices, centroid) != 0) { + for (const auto & center_point : center_points) { + const double ego_to_obstacle_distance = + std::hypot(center_point.x - centroid.x(), center_point.y - centroid.y()); + if (ego_to_obstacle_distance < p.pointcloud_search_radius) { + geometry_msgs::msg::Point obstacle_position; + obstacle_position.x = centroid.x(); + obstacle_position.y = centroid.y(); + MultiPoint2d obstacle_rel_points; + for (const auto & index : cluster_indices.indices) { + const auto & point = filtered_points_ptr->points.at(index); + bg::append( + obstacle_rel_points, Point2d(point.x - centroid.x(), point.y - centroid.y())); + } + Polygon2d obstacle_footprint; + bg::convex_hull(obstacle_rel_points, obstacle_footprint); + const double lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, obstacle_position); + target_obstacles.emplace_back( + pointcloud.header.stamp, obstacle_position, toGeomPoly(obstacle_footprint), + ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + break; + } } } } From 06176a8b5c42a814b74ac2d6c7f40cb2537c78cd Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 8 May 2024 12:18:45 +0900 Subject: [PATCH 25/64] add parameters for clustering --- .../include/obstacle_cruise_planner/node.hpp | 3 ++ planning/obstacle_cruise_planner/src/node.cpp | 29 +++++++++++++++++-- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 46d2f58b7eb9..4dd201b54998 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -193,6 +193,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double pointcloud_voxel_grid_x; double pointcloud_voxel_grid_y; double pointcloud_voxel_grid_z; + double pointcloud_cluster_tolerance; + int pointcloud_min_cluster_size; + int pointcloud_max_cluster_size; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 7c0b0b589e5f..0620194bd4fc 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -276,6 +276,12 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); pointcloud_voxel_grid_z = node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); + pointcloud_cluster_tolerance = + node.declare_parameter("behavior_determination.pointcloud_cluster_tolerance"); + pointcloud_min_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_min_cluster_size"); + pointcloud_max_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_max_cluster_size"); obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( @@ -333,6 +339,23 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.use_pointcloud", use_pointcloud); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_cluster_tolerance", + pointcloud_cluster_tolerance); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size); tier4_autoware_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); @@ -733,9 +756,9 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( tree->setInputCloud(filtered_points_ptr); std::vector clusters; pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(1); - ec.setMinClusterSize(1); - ec.setMaxClusterSize(100000); + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); ec.setSearchMethod(tree); ec.setInputCloud(filtered_points_ptr); ec.extract(clusters); From 98bcd0856f861d23c9f7989d8128939ec0b3e27c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 16 May 2024 15:26:17 +0900 Subject: [PATCH 26/64] add max_num_points parameter to dummy object --- .../include/dummy_perception_publisher/node.hpp | 1 + simulator/dummy_perception_publisher/msg/Object.msg | 1 + simulator/dummy_perception_publisher/src/node.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 770663e84dc0..b5deb5bbbb63 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -53,6 +53,7 @@ struct ObjectInfo double std_dev_y; double std_dev_z; double std_dev_yaw; + std::size_t max_num_points; tf2::Transform tf_map2moved_object; // pose and twist geometry_msgs::msg::TwistWithCovariance twist_covariance_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 30279299dbfb..22861b41080c 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/simulator/dummy_perception_publisher/msg/Object.msg @@ -5,6 +5,7 @@ autoware_auto_perception_msgs/ObjectClassification classification autoware_auto_perception_msgs/Shape shape float32 max_velocity float32 min_velocity +uint32 max_num_points uint8 ADD=0 uint8 MODIFY=1 diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 3ac663d2c864..1821133f999b 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -45,6 +45,7 @@ ObjectInfo::ObjectInfo( std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])), std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])), std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])), + max_num_points(object.max_num_points), twist_covariance_(object.initial_state.twist_covariance), pose_covariance_(object.initial_state.pose_covariance) { From fb00b59d8f14cec6810e7fab12bc34d8a0c617c7 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 16 May 2024 15:27:07 +0900 Subject: [PATCH 27/64] downsample pointcloud when the number of points is larger than max_num_points --- .../src/pointcloud_creator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 05354fa0a966..8353bc1b3cc7 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -253,6 +254,18 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr } } + for (size_t i = 0; i < pointclouds.size(); ++i) { + auto & cloud = pointclouds.at(i); + const auto max_num_points = obj_infos.at(i).max_num_points; + if (max_num_points != 0 && cloud->size() > max_num_points) { + pcl::Indices indices; + pcl::ExtractIndices filter; + filter.setInputCloud(cloud); + filter.setIndices(0, cloud->size() / 2 - max_num_points / 2, 1, max_num_points); + filter.filter(*cloud); + } + } + for (const auto & cloud : pointclouds) { for (const auto & pt : *cloud) { merged_pointcloud->push_back(pt); From 5f9e4ab5ae7d8d46521c736b1d259040121f3bc5 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 16 May 2024 15:27:44 +0900 Subject: [PATCH 28/64] add max_num_points property to dummy bus --- common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp | 4 ++++ .../src/tools/interactive_object.hpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp index 49da06aa2478..3e66935ac7a2 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -189,6 +189,8 @@ BusInitialPoseTool::BusInitialPoseTool() "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer()); min_velocity_ = new rviz_common::properties::FloatProperty( "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer()); + max_num_points_ = new rviz_common::properties::IntProperty( + "Max number of points", 0, "Max number of points in point cloud", getPropertyContainer()); std_dev_x_->setMin(0); std_dev_y_->setMin(0); std_dev_z_->setMin(0); @@ -235,6 +237,8 @@ Object BusInitialPoseTool::createObjectMsg() const object.initial_state.pose_covariance.covariance[35] = std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + object.max_num_points = max_num_points_->getInt(); + std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index fc40b08be822..a20fd62ee60a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -163,6 +164,7 @@ protected Q_SLOTS: rviz_common::properties::FloatProperty * max_velocity_; rviz_common::properties::FloatProperty * min_velocity_; rviz_common::properties::FloatProperty * accel_; + rviz_common::properties::IntProperty * max_num_points_; rviz_common::properties::TfFrameProperty * property_frame_; private: From 0589073a4ab8b7a925ad495151405d3d946bfc24 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 16 May 2024 17:01:19 +0900 Subject: [PATCH 29/64] add parameters for pointcloud based obstacles --- .../config/obstacle_cruise_planner.param.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index bd9c5750af78..604c817d05fb 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -37,6 +37,7 @@ motorcycle: true bicycle: true pedestrian: true + pointcloud: false cruise_obstacle_type: inside: @@ -68,8 +69,10 @@ motorcycle: true bicycle: true pedestrian: true + pointcloud: false behavior_determination: + use_pointcloud: false decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking prediction_resampling_time_interval: 0.1 From 7f9738e33637ceb902e8fda60560d0352ef0218f Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 22 May 2024 13:03:12 +0900 Subject: [PATCH 30/64] store pointcloud in obstacle struct --- .../common_structs.hpp | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index e98b0436c17d..459fdcf3599b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -75,28 +75,26 @@ struct Obstacle } Obstacle( - const rclcpp::Time & arg_stamp, const geometry_msgs::msg::Point & position, - const geometry_msgs::msg::Polygon & footprint, const double ego_to_obstacle_distance, - const double lat_dist_from_obstacle_to_traj) + const rclcpp::Time & arg_stamp, const PointCloud::Ptr & cloud, + const pcl::PointXYZ & front_point, const pcl::PointXYZ & back_point, + const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), - orientation_reliable(false), - twist_reliable(false), ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + cloud(cloud), + front_point(front_point), + back_point(back_point) { - pose.position = position; - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = 0; - pose.orientation.w = 1; - - shape.type = Shape::POLYGON; - shape.footprint = footprint; } + // available for PredictedObject only Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. + double ego_to_obstacle_distance; + double lat_dist_from_obstacle_to_traj; + + // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp bool orientation_reliable; Twist twist; @@ -105,8 +103,11 @@ struct Obstacle std::string uuid; Shape shape; std::vector predicted_paths; - double ego_to_obstacle_distance; - double lat_dist_from_obstacle_to_traj; + + // for PointCloud + PointCloud::Ptr cloud; + pcl::PointXYZ front_point; + pcl::PointXYZ back_point; }; struct TargetObstacleInterface From 8784f5908443286365d075b14501a0792ee2c28c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 22 May 2024 13:05:50 +0900 Subject: [PATCH 31/64] change obstacle conversion method --- planning/obstacle_cruise_planner/src/node.cpp | 105 +++++++++++++----- 1 file changed, 78 insertions(+), 27 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 0620194bd4fc..1f54abb979d0 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -28,7 +28,9 @@ #include #include +#include #include +#include #include #include #include @@ -224,6 +226,15 @@ std::vector resampleTrajectoryPoints( return motion_utils::convertToTrajectoryPointArray(resampled_traj); } +geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x; + geom_point.y = point.y; + geom_point.z = point.z; + return geom_point; +} + geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; @@ -724,7 +735,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( if (p.use_pointcloud) { const auto & pointcloud = pointcloud_sub_.getData(); - // transform pointcloud + // 1. transform pointcloud std::optional transform_stamped{}; try { transform_stamped = tf_buffer_->lookupTransform( @@ -745,6 +756,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( PointCloud::Ptr transformed_points_ptr(new PointCloud); pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + // 2. downsample & cluster pointcloud PointCloud::Ptr filtered_points_ptr(new PointCloud); pcl::VoxelGrid filter; filter.setInputCloud(transformed_points_ptr); @@ -768,34 +780,73 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); } - // search obstacle candidate pointcloud to reduce calculation cost for (const auto & cluster_indices : clusters) { - Eigen::Vector4d centroid; - if (pcl::compute3DCentroid(*filtered_points_ptr, cluster_indices, centroid) != 0) { - for (const auto & center_point : center_points) { - const double ego_to_obstacle_distance = - std::hypot(center_point.x - centroid.x(), center_point.y - centroid.y()); - if (ego_to_obstacle_distance < p.pointcloud_search_radius) { - geometry_msgs::msg::Point obstacle_position; - obstacle_position.x = centroid.x(); - obstacle_position.y = centroid.y(); - MultiPoint2d obstacle_rel_points; - for (const auto & index : cluster_indices.indices) { - const auto & point = filtered_points_ptr->points.at(index); - bg::append( - obstacle_rel_points, Point2d(point.x - centroid.x(), point.y - centroid.y())); + // 3. filter clusters + pcl::IndicesConstPtr cluster_indices_ptr = + pcl::make_shared(cluster_indices.indices); + PointCloud::Ptr cluster_points_ptr(new PointCloud); + pcl::ExtractIndices extract; + extract.setInputCloud(filtered_points_ptr); + extract.setIndices(cluster_indices_ptr); + extract.filter(*cluster_points_ptr); + + std::optional ego_to_obstacle_distance = std::nullopt; + pcl::PointXYZ front_obstacle_point; + pcl::PointXYZ back_obstacle_point; + double min_obstacle_distance = std::numeric_limits::max(); + std::optional closest_obstacle_point_index = std::nullopt; + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cluster_points_ptr); + + for (const auto & center_point : center_points) { + pcl::PointXYZ point(center_point.x, center_point.y, 0.0); + pcl::Indices index(1); + std::vector sqr_distance(1); + if (kdtree.nearestKSearch(point, 1, index, sqr_distance) > 0) { + if (!ego_to_obstacle_distance) { + // Check if the obstacle is in front of the ego. + const auto ego_idx = + ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); + ego_to_obstacle_distance = calcDistanceToFrontVehicle( + traj_points, ego_idx, toGeomPoint(cluster_points_ptr->points[index.front()])); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle since it is not front obstacle."); + break; } - Polygon2d obstacle_footprint; - bg::convex_hull(obstacle_rel_points, obstacle_footprint); - const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, obstacle_position); - target_obstacles.emplace_back( - pointcloud.header.stamp, obstacle_position, toGeomPoly(obstacle_footprint), - ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); - break; + front_obstacle_point = cluster_points_ptr->points[index.front()]; + } else { + back_obstacle_point = cluster_points_ptr->points[index.front()]; + } + const auto min_distance = std::sqrt(sqr_distance.front()); + if (min_distance < p.pointcloud_search_radius && min_distance < min_obstacle_distance) { + min_obstacle_distance = min_distance; + closest_obstacle_point_index = index.front(); } } } + + if (closest_obstacle_point_index) { + const auto closest_obstacle_point = + toGeomPoint(cluster_points_ptr->points[*closest_obstacle_point_index]); + + // 4. Check if lateral distance is smaller than the threshold + const auto lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, closest_obstacle_point); + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_slow_down); + if (max_lat_margin < lat_dist_from_obstacle_to_traj) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle since it is too far from the trajectory."); + continue; + } + + target_obstacles.emplace_back( + pointcloud.header.stamp, cluster_points_ptr, front_obstacle_point, back_obstacle_point, + *ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + } } } } else { @@ -931,9 +982,9 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); - for (const auto & traj_poly : decimated_traj_polys) { - const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); - precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); + precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); } if (!p.use_pointcloud) { From 999605b6d20f8ef16027f5581b49eb714412b5d1 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 12:20:44 +0900 Subject: [PATCH 32/64] migrate previous changes to new package --- .../common_structs.hpp | 6 +- .../autoware_obstacle_cruise_planner/node.hpp | 26 + .../type_alias.hpp | 7 + .../src/node.cpp | 484 ++++++++++++++---- 4 files changed, 431 insertions(+), 92 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp index 01a1bc999ed8..8e09d00833cb 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp @@ -58,15 +58,15 @@ struct Obstacle const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), uuid(tier4_autoware_utils::toHexString(object.object_id)), - shape(object.shape), - ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) + shape(object.shape) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp index dcea7e1dc257..a68d411eb490 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp @@ -27,6 +27,9 @@ #include #include +#include +#include + #include #include #include @@ -58,10 +61,18 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, + const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PointCloud2 & /* pointcloud */, + const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; std::optional createStopObstacle( @@ -126,6 +137,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -149,9 +162,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/odometry"}; tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber pointcloud_sub_{ + this, "~/input/pointcloud"}; tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + // Vehicle Parameters VehicleInfo vehicle_info_; @@ -182,6 +200,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onParam(const std::vector & parameters); double decimate_trajectory_step_length; + bool use_pointcloud{false}; + double pointcloud_search_radius; + double pointcloud_voxel_grid_x; + double pointcloud_voxel_grid_y; + double pointcloud_voxel_grid_z; + double pointcloud_cluster_tolerance; + int pointcloud_min_cluster_size; + int pointcloud_max_cluster_size; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp index 7b435acb5f13..b9993fc0ed06 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/type_alias.hpp @@ -27,6 +27,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_planning_msgs/msg/stop_factor.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" @@ -35,6 +36,9 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include + using autoware::vehicle_info_utils::VehicleInfo; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; @@ -50,6 +54,7 @@ using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -63,4 +68,6 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using PointCloud = pcl::PointCloud; + #endif // AUTOWARE_OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 18ea1243d910..76afd1b10363 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -23,8 +23,19 @@ #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" +#include +#include + #include +#include +#include +#include +#include +#include +#include +#include + #include #include @@ -54,6 +65,22 @@ std::optional getObstacleFromUuid( return *itr; } +geometry_msgs::msg::Pose getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + const auto yaw = tier4_autoware_utils::getRPY(base_pose).z; + + geometry_msgs::msg::Pose center_pose; + center_pose.position.x = + base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} + std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) @@ -191,6 +218,15 @@ std::vector resampleTrajectoryPoints( return motion_utils::convertToTrajectoryPointArray(resampled_traj); } +geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x; + geom_point.y = point.y; + geom_point.z = point.z; + return geom_point; +} + geometry_msgs::msg::Point toGeomPoint(const tier4_autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; @@ -222,6 +258,21 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara { // behavior determination decimate_trajectory_step_length = node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); + use_pointcloud = node.declare_parameter("behavior_determination.use_pointcloud"); + pointcloud_search_radius = + node.declare_parameter("behavior_determination.pointcloud_search_radius"); + pointcloud_voxel_grid_x = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_x"); + pointcloud_voxel_grid_y = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); + pointcloud_voxel_grid_z = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); + pointcloud_cluster_tolerance = + node.declare_parameter("behavior_determination.pointcloud_cluster_tolerance"); + pointcloud_min_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_min_cluster_size"); + pointcloud_max_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_max_cluster_size"); obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( @@ -281,6 +332,23 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.use_pointcloud", use_pointcloud); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_cluster_tolerance", + pointcloud_cluster_tolerance); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size); tier4_autoware_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); @@ -384,6 +452,10 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_slow_down_planning_info_pub_ = create_publisher("~/debug/slow_down_planning_info", 1); + // tf listener + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + const auto longitudinal_info = LongitudinalInfo(*this); ego_nearest_param_ = EgoNearestParam(*this); @@ -431,6 +503,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & outside_cruise_obstacle_types_ = getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); + use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = + declare_parameter("common.slow_down_obstacle_type.pointcloud"); } // set parameter callback @@ -489,15 +564,18 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { + const auto & p = behavior_determination_param_; const auto ego_odom_ptr = ego_odom_sub_.takeData(); const auto objects_ptr = objects_sub_.takeData(); + const auto pointcloud_ptr = pointcloud_sub_.takeData(); const auto acc_ptr = acc_sub_.takeData(); - if (!ego_odom_ptr || !objects_ptr || !acc_ptr) { + if ( + !ego_odom_ptr || (!p.use_pointcloud && !objects_ptr) || (p.use_pointcloud && !pointcloud_ptr) || + !acc_ptr) { return; } const auto & ego_odom = *ego_odom_ptr; - const auto & objects = *objects_ptr; const auto & acc = *acc_ptr; const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); @@ -516,11 +594,24 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points); + const auto target_obstacles = [&]() { + if (p.use_pointcloud) { + return convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); + } else { + return convertToObstacles(ego_odom, *objects_ptr, traj_points); + } + }(); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. - const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); + const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { + if (p.use_pointcloud) { + return determineEgoBehaviorAgainstObstacles( + ego_odom, *pointcloud_ptr, traj_points, target_obstacles); + } else { + return determineEgoBehaviorAgainstObstacles( + ego_odom, *objects_ptr, traj_points, target_obstacles); + } + }(); // 3. Create data for planning const auto planner_data = createPlannerData(ego_odom, acc, traj_points); @@ -710,6 +801,138 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( return target_obstacles; } +std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const +{ + stop_watch_.tic(__func__); + + const auto & p = behavior_determination_param_; + + std::vector target_obstacles; + + // 1. transform pointcloud + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; + } + + if (transform_stamped && !pointcloud.data.empty()) { + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, pointcloud, transformed_points); + PointCloud::Ptr transformed_points_ptr(new PointCloud); + pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + + // 2. downsample & cluster pointcloud + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(transformed_points_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + std::vector center_points(traj_points.size()); + for (const auto & traj_point : traj_points) { + center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); + } + + for (const auto & cluster_indices : clusters) { + // 3. filter clusters + pcl::IndicesConstPtr cluster_indices_ptr = + pcl::make_shared(cluster_indices.indices); + PointCloud::Ptr cluster_points_ptr(new PointCloud); + pcl::ExtractIndices extract; + extract.setInputCloud(filtered_points_ptr); + extract.setIndices(cluster_indices_ptr); + extract.filter(*cluster_points_ptr); + + std::optional ego_to_obstacle_distance = std::nullopt; + pcl::PointXYZ front_obstacle_point; + pcl::PointXYZ back_obstacle_point; + double min_obstacle_distance = std::numeric_limits::max(); + std::optional closest_obstacle_point_index = std::nullopt; + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cluster_points_ptr); + + for (const auto & center_point : center_points) { + pcl::PointXYZ point(center_point.x, center_point.y, 0.0); + pcl::Indices index(1); + std::vector sqr_distance(1); + if (kdtree.nearestKSearch(point, 1, index, sqr_distance) > 0) { + if (!ego_to_obstacle_distance) { + // Check if the obstacle is in front of the ego. + const auto ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + ego_to_obstacle_distance = calcDistanceToFrontVehicle( + traj_points, ego_idx, toGeomPoint(cluster_points_ptr->points[index.front()])); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle since it is not front obstacle."); + break; + } + front_obstacle_point = cluster_points_ptr->points[index.front()]; + } else { + back_obstacle_point = cluster_points_ptr->points[index.front()]; + } + const auto min_distance = std::sqrt(sqr_distance.front()); + if (min_distance < p.pointcloud_search_radius && min_distance < min_obstacle_distance) { + min_obstacle_distance = min_distance; + closest_obstacle_point_index = index.front(); + } + } + } + + if (closest_obstacle_point_index) { + const auto closest_obstacle_point = + toGeomPoint(cluster_points_ptr->points[*closest_obstacle_point_index]); + + // 4. Check if lateral distance is smaller than the threshold + const auto lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, closest_obstacle_point); + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_slow_down); + if (max_lat_margin < lat_dist_from_obstacle_to_traj) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle since it is too far from the trajectory."); + continue; + } + + target_obstacles.emplace_back( + pointcloud.header.stamp, cluster_points_ptr, front_obstacle_point, back_obstacle_point, + *ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + } + } + } + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, + calculation_time); + + return target_obstacles; +} + bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const { const auto & types = stop_obstacle_types_; @@ -836,6 +1059,63 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; } +std::tuple, std::vector, std::vector> +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PointCloud2 & /* pointcloud */, + const std::vector & traj_points, const std::vector & obstacles) +{ + stop_watch_.tic(__func__); + + // calculated decimated trajectory points and trajectory polygon + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); + const auto decimated_traj_polys = + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); + debug_data_ptr_->detection_polygons = decimated_traj_polys; + + // determine ego's behavior from stop, cruise and slow down + std::vector stop_obstacles; + std::vector cruise_obstacles; + std::vector slow_down_obstacles; + slow_down_condition_counter_.resetCurrentUuids(); + for (const auto & obstacle : obstacles) { + const auto obstacle_poly = obstacle.toPolygon(); + + // Calculate distance between trajectory and obstacle first + double precise_lat_dist = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); + precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); + } + + // Filter obstacles for stop and slow down + const auto stop_obstacle = createStopObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + if (stop_obstacle) { + stop_obstacles.push_back(*stop_obstacle); + continue; + } + const auto slow_down_obstacle = + createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + slow_down_condition_counter_.removeCounterUnlessUpdated(); + + // update previous obstacles + prev_stop_obstacles_ = stop_obstacles; + prev_cruise_obstacles_ = cruise_obstacles; + prev_slow_down_obstacles_ = slow_down_obstacles; + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); + + return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; +} + std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const { @@ -1176,7 +1456,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: consider all target obstacles when driving backward - if (!isStopObstacle(obstacle.classification.label)) { + if ( + (!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) || + (p.use_pointcloud && !use_pointcloud_for_stop_)) { return std::nullopt; } @@ -1189,56 +1471,58 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } - // check obstacle velocity - // NOTE: If precise_lat_dist is 0, always plan stop - constexpr double epsilon = 1e-6; - if (epsilon < precise_lat_dist) { - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { - return std::nullopt; + if (!p.use_pointcloud) { + // check obstacle velocity + // NOTE: If precise_lat_dist is 0, always plan stop + constexpr double epsilon = 1e-6; + if (epsilon < precise_lat_dist) { + const auto [obstacle_tangent_vel, obstacle_normal_vel] = + projectObstacleVelocityToTrajectory(traj_points, obstacle); + if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { + return std::nullopt; + } } - } - // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, - // and the collision between ego and obstacles are within the margin threshold. - const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - if (is_obstacle_crossing && has_high_speed) { - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( - obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); - - std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); - if (collision_points.empty()) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " - "predicted path " - "and the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; - } + // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, + // and the collision between ego and obstacles are within the margin threshold. + const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); + const double has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + if (is_obstacle_crossing && has_high_speed) { + // Get highest confidence predicted path + const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon); + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } - const double collision_time_margin = - calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); - if (p.collision_time_margin < collision_time_margin) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; + const double collision_time_margin = + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } } } @@ -1252,7 +1536,12 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + const auto [tangent_vel, normal_vel] = [&]() { + if (p.use_pointcloud) { + return std::make_pair(0., 0.); + } + return projectObstacleVelocityToTrajectory(traj_points, obstacle); + }(); return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, collision_point->first, collision_point->second}; @@ -1264,49 +1553,61 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; - slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); - const bool is_prev_obstacle_slow_down = - getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + if (!p.use_pointcloud) { + slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); + } - if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { + if ( + !enable_slow_down_planning_ || + (!p.use_pointcloud && !isSlowDownObstacle(obstacle.classification.label)) || + (p.use_pointcloud && !use_pointcloud_for_slow_down_)) { return std::nullopt; } - // check lateral distance considering hysteresis - const bool is_lat_dist_low = isLowerConsideringHysteresis( - precise_lat_dist, is_prev_obstacle_slow_down, - p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, - p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); - - const bool is_slow_down_required = [&]() { - if (is_prev_obstacle_slow_down) { - // check if exiting slow down - if (!is_lat_dist_low) { - const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); - if (count <= -p.successive_num_to_exit_slow_down_condition) { - slow_down_condition_counter_.reset(obstacle.uuid); - return false; - } - } - return true; + if (p.use_pointcloud) { + if (p.max_lat_margin_for_slow_down < precise_lat_dist) { + return std::nullopt; } - // check if entering slow down - if (is_lat_dist_low) { - const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); - if (p.successive_num_to_entry_slow_down_condition <= count) { - slow_down_condition_counter_.reset(obstacle.uuid); + } else { + const bool is_prev_obstacle_slow_down = + getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = isLowerConsideringHysteresis( + precise_lat_dist, is_prev_obstacle_slow_down, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, + p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obstacle.uuid); + return false; + } + } return true; } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obstacle.uuid); + return true; + } + } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", + object_id.c_str(), precise_lat_dist); + return std::nullopt; } - return false; - }(); - if (!is_slow_down_required) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), - precise_lat_dist); - return std::nullopt; } const auto obstacle_poly = obstacle.toPolygon(); @@ -1376,7 +1677,12 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + const auto [tangent_vel, normal_vel] = [&]() { + if (p.use_pointcloud) { + return std::make_pair(0., 0.); + } + return projectObstacleVelocityToTrajectory(traj_points, obstacle); + }(); return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, normal_vel, precise_lat_dist, front_collision_point, back_collision_point}; From a22ef49912c244e365a53f9fbbad23efcf357227 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 16:15:47 +0900 Subject: [PATCH 33/64] store necessary points only --- .../common_structs.hpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp index 8e09d00833cb..2ac55bbdf985 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/common_structs.hpp @@ -75,21 +75,20 @@ struct Obstacle } Obstacle( - const rclcpp::Time & arg_stamp, const PointCloud::Ptr & cloud, - const pcl::PointXYZ & front_point, const pcl::PointXYZ & back_point, + const rclcpp::Time & arg_stamp, + const std::optional & stop_collision_point, + const std::optional & slow_down_front_collision_point, + const std::optional & slow_down_back_collision_point, const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), - cloud(cloud), - front_point(front_point), - back_point(back_point) + stop_collision_point(stop_collision_point), + slow_down_front_collision_point(slow_down_front_collision_point), + slow_down_back_collision_point(slow_down_back_collision_point) { } - // available for PredictedObject only - Polygon2d toPolygon() const { return tier4_autoware_utils::toPolygon2d(pose, shape); } - rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; @@ -105,9 +104,9 @@ struct Obstacle std::vector predicted_paths; // for PointCloud - PointCloud::Ptr cloud; - pcl::PointXYZ front_point; - pcl::PointXYZ back_point; + std::optional stop_collision_point; + std::optional slow_down_front_collision_point; + std::optional slow_down_back_collision_point; }; struct TargetObstacleInterface From 98ea643cffe747005f8cb4677d27a47f3592b775 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 16:20:11 +0900 Subject: [PATCH 34/64] move use_pointcloud to common parameter --- .../autoware_obstacle_cruise_planner/node.hpp | 4 +- .../src/node.cpp | 47 ++++++++++++------- 2 files changed, 33 insertions(+), 18 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp index a68d411eb490..5d171df3f923 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/node.hpp @@ -127,6 +127,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; + bool use_pointcloud_{false}; + bool enable_slow_down_planning_{false}; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; double additional_safe_distance_margin_on_curve_; @@ -200,7 +202,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onParam(const std::vector & parameters); double decimate_trajectory_step_length; - bool use_pointcloud{false}; double pointcloud_search_radius; double pointcloud_voxel_grid_x; double pointcloud_voxel_grid_y; @@ -296,7 +297,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; - bool enable_slow_down_planning_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 76afd1b10363..5f34547b82f0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -258,7 +258,6 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara { // behavior determination decimate_trajectory_step_length = node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); - use_pointcloud = node.declare_parameter("behavior_determination.use_pointcloud"); pointcloud_search_radius = node.declare_parameter("behavior_determination.pointcloud_search_radius"); pointcloud_voxel_grid_x = @@ -332,8 +331,6 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); - tier4_autoware_utils::updateParam( - parameters, "behavior_determination.use_pointcloud", use_pointcloud); tier4_autoware_utils::updateParam( parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); tier4_autoware_utils::updateParam( @@ -462,6 +459,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & enable_debug_info_ = declare_parameter("common.enable_debug_info"); enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info"); + use_pointcloud_ = declare_parameter("common.use_pointcloud"); enable_slow_down_planning_ = declare_parameter("common.enable_slow_down_planning"); behavior_determination_param_ = BehaviorDeterminationParam(*this); @@ -537,6 +535,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( tier4_autoware_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + tier4_autoware_utils::updateParam(parameters, "common.use_pointcloud", use_pointcloud_); + tier4_autoware_utils::updateParam( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); tier4_autoware_utils::updateParam( @@ -564,13 +564,12 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto & p = behavior_determination_param_; const auto ego_odom_ptr = ego_odom_sub_.takeData(); const auto objects_ptr = objects_sub_.takeData(); const auto pointcloud_ptr = pointcloud_sub_.takeData(); const auto acc_ptr = acc_sub_.takeData(); if ( - !ego_odom_ptr || (!p.use_pointcloud && !objects_ptr) || (p.use_pointcloud && !pointcloud_ptr) || + !ego_odom_ptr || (!use_pointcloud_ && !objects_ptr) || (use_pointcloud_ && !pointcloud_ptr) || !acc_ptr) { return; } @@ -595,7 +594,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (2) in front of ego // (3) not too far from trajectory const auto target_obstacles = [&]() { - if (p.use_pointcloud) { + if (use_pointcloud_) { return convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); } else { return convertToObstacles(ego_odom, *objects_ptr, traj_points); @@ -604,7 +603,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { - if (p.use_pointcloud) { + if (use_pointcloud_) { return determineEgoBehaviorAgainstObstacles( ego_odom, *pointcloud_ptr, traj_points, target_obstacles); } else { @@ -1145,9 +1144,14 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) { - const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; + if (use_pointcloud_) { + return std::nullopt; + } + + const auto & object_id = obstacle.uuid.substr(0, 4); + // NOTE: When driving backward, Stop will be planned instead of cruise. // When the obstacle is crossing the ego's trajectory, cruise can be ignored. if (!isCruiseObstacle(obstacle.classification.label) || !is_driving_forward_) { @@ -1188,10 +1192,15 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points) { + const auto & p = behavior_determination_param_; + + if (use_pointcloud_) { + return std::nullopt; + } + if (traj_points.empty()) return std::nullopt; // check label const auto & object_id = obstacle.uuid.substr(0, 4); - const auto & p = behavior_determination_param_; if (!isOutsideCruiseObstacle(obstacle.classification.label)) { RCLCPP_INFO_EXPRESSION( @@ -1457,8 +1466,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // NOTE: consider all target obstacles when driving backward if ( - (!p.use_pointcloud && !isStopObstacle(obstacle.classification.label)) || - (p.use_pointcloud && !use_pointcloud_for_stop_)) { + (!use_pointcloud_ && !isStopObstacle(obstacle.classification.label)) || + (use_pointcloud_ && !use_pointcloud_for_stop_)) { return std::nullopt; } @@ -1537,7 +1546,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = [&]() { - if (p.use_pointcloud) { + if (use_pointcloud_) { return std::make_pair(0., 0.); } return projectObstacleVelocityToTrajectory(traj_points, obstacle); @@ -1554,14 +1563,14 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; - if (!p.use_pointcloud) { + if (!use_pointcloud_) { slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); } if ( !enable_slow_down_planning_ || - (!p.use_pointcloud && !isSlowDownObstacle(obstacle.classification.label)) || - (p.use_pointcloud && !use_pointcloud_for_slow_down_)) { + (!use_pointcloud_ && !isSlowDownObstacle(obstacle.classification.label)) || + (use_pointcloud_ && !use_pointcloud_for_slow_down_)) { return std::nullopt; } @@ -1678,7 +1687,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = [&]() { - if (p.use_pointcloud) { + if (use_pointcloud_) { return std::make_pair(0., 0.); } return projectObstacleVelocityToTrajectory(traj_points, obstacle); @@ -1692,6 +1701,12 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { + const auto & p = behavior_determination_param_; + + if (use_pointcloud_) { + return; + } + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), From 3cb86753b3e885e371ae061d0f74b133faec9fb7 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 16:25:40 +0900 Subject: [PATCH 35/64] extract necessary points from pointcloud --- .../src/node.cpp | 140 ++++++++++-------- 1 file changed, 75 insertions(+), 65 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 5f34547b82f0..3b1f61ca0601 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -855,8 +855,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); } + // 3. convert clusters to obstacles for (const auto & cluster_indices : clusters) { - // 3. filter clusters pcl::IndicesConstPtr cluster_indices_ptr = pcl::make_shared(cluster_indices.indices); PointCloud::Ptr cluster_points_ptr(new PointCloud); @@ -865,61 +865,59 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( extract.setIndices(cluster_indices_ptr); extract.filter(*cluster_points_ptr); - std::optional ego_to_obstacle_distance = std::nullopt; - pcl::PointXYZ front_obstacle_point; - pcl::PointXYZ back_obstacle_point; - double min_obstacle_distance = std::numeric_limits::max(); - std::optional closest_obstacle_point_index = std::nullopt; pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(cluster_points_ptr); + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_slow_down); + double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); + std::optional ego_to_obstacle_distance = std::nullopt; + std::optional stop_collision_point = std::nullopt; + std::optional slow_down_front_collision_point = std::nullopt; + std::optional slow_down_back_collision_point = std::nullopt; + for (const auto & center_point : center_points) { pcl::PointXYZ point(center_point.x, center_point.y, 0.0); pcl::Indices index(1); std::vector sqr_distance(1); if (kdtree.nearestKSearch(point, 1, index, sqr_distance) > 0) { + const auto nearest_obstacle_point = + toGeomPoint(cluster_points_ptr->points[index.front()]); + const auto current_lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, nearest_obstacle_point) - + vehicle_info_.vehicle_width_m; + + if (current_lat_dist_from_obstacle_to_traj < max_lat_margin) { if (!ego_to_obstacle_distance) { - // Check if the obstacle is in front of the ego. - const auto ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - ego_to_obstacle_distance = calcDistanceToFrontVehicle( - traj_points, ego_idx, toGeomPoint(cluster_points_ptr->points[index.front()])); - if (!ego_to_obstacle_distance) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle since it is not front obstacle."); - break; + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); + } + if (current_lat_dist_from_obstacle_to_traj < p.max_lat_margin_for_stop) { + if (!stop_collision_point) { + stop_collision_point = nearest_obstacle_point; + } } - front_obstacle_point = cluster_points_ptr->points[index.front()]; + if (current_lat_dist_from_obstacle_to_traj < p.max_lat_margin_for_slow_down) { + if (!slow_down_front_collision_point) { + slow_down_front_collision_point = nearest_obstacle_point; + } else { + slow_down_back_collision_point = nearest_obstacle_point; + } + } + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); } else { - back_obstacle_point = cluster_points_ptr->points[index.front()]; - } - const auto min_distance = std::sqrt(sqr_distance.front()); - if (min_distance < p.pointcloud_search_radius && min_distance < min_obstacle_distance) { - min_obstacle_distance = min_distance; - closest_obstacle_point_index = index.front(); - } - } - } - - if (closest_obstacle_point_index) { - const auto closest_obstacle_point = - toGeomPoint(cluster_points_ptr->points[*closest_obstacle_point_index]); - - // 4. Check if lateral distance is smaller than the threshold - const auto lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, closest_obstacle_point); - const auto max_lat_margin = - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_slow_down); - if (max_lat_margin < lat_dist_from_obstacle_to_traj) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle since it is too far from the trajectory."); continue; + } } + if (ego_to_obstacle_distance) { target_obstacles.emplace_back( - pointcloud.header.stamp, cluster_points_ptr, front_obstacle_point, back_obstacle_point, - *ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, *ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj); + } } } } @@ -994,7 +992,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = obstacle.toPolygon(); + const auto obstacle_poly = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); @@ -1077,14 +1075,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = obstacle.toPolygon(); - - // Calculate distance between trajectory and obstacle first - double precise_lat_dist = std::numeric_limits::max(); - for (const auto & traj_poly : decimated_traj_polys) { - const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); - precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); - } + const double precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; // Filter obstacles for stop and slow down const auto stop_obstacle = createStopObstacle( @@ -1480,7 +1471,17 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } - if (!p.use_pointcloud) { + std::optional> collision_point = std::nullopt; + if (use_pointcloud_) { + if (obstacle.stop_collision_point) { + const auto dist_to_collide_on_traj = + motion_utils::calcSignedArcLength(traj_points, 0, obstacle.stop_collision_point.value()); + collision_point = + std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); + } + } else { + const auto & object_id = obstacle.uuid.substr(0, 4); + // check obstacle velocity // NOTE: If precise_lat_dist is 0, always plan stop constexpr double epsilon = 1e-6; @@ -1531,16 +1532,16 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( object_id.c_str()); debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); return std::nullopt; - } } } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = - createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); - const auto collision_point = polygon_utils::getCollisionPoint( + collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); + } if (!collision_point) { return std::nullopt; } @@ -1574,9 +1575,16 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } - if (p.use_pointcloud) { - if (p.max_lat_margin_for_slow_down < precise_lat_dist) { - return std::nullopt; + std::optional front_collision_point = std::nullopt; + std::optional back_collision_point = std::nullopt; + if (use_pointcloud_) { + if (obstacle.slow_down_front_collision_point) { + front_collision_point = obstacle.slow_down_front_collision_point; + if (obstacle.slow_down_back_collision_point) { + back_collision_point = obstacle.slow_down_back_collision_point; + } else { + back_collision_point = front_collision_point; + } } } else { const bool is_prev_obstacle_slow_down = @@ -1616,10 +1624,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), precise_lat_dist); return std::nullopt; - } } - const auto obstacle_poly = obstacle.toPolygon(); + const auto obstacle_poly = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. @@ -1658,7 +1665,6 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate front collision point double front_min_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point front_collision_point; for (const auto & collision_poly : front_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); @@ -1673,7 +1679,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate back collision point double back_max_dist = -std::numeric_limits::max(); - geometry_msgs::msg::Point back_collision_point = front_collision_point; + back_collision_point = front_collision_point; for (const auto & collision_poly : back_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); @@ -1684,6 +1690,10 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl back_collision_point = collision_geom_point; } } + } + } + if (!front_collision_point || !back_collision_point) { + return std::nullopt; } const auto [tangent_vel, normal_vel] = [&]() { @@ -1692,9 +1702,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } return projectObstacleVelocityToTrajectory(traj_points, obstacle); }(); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, tangent_vel, normal_vel, - precise_lat_dist, front_collision_point, back_collision_point}; + return SlowDownObstacle{ + obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, + normal_vel, precise_lat_dist, *front_collision_point, *back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( From a6d174397b1a7a277aa48c7fba2fbbb7492f00de Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 16:26:15 +0900 Subject: [PATCH 36/64] add use_pointcloud parameter to planner interface --- .../planner_interface.hpp | 6 +- .../src/node.cpp | 124 +++++++++--------- .../src/planner_interface.cpp | 2 +- 3 files changed, 67 insertions(+), 65 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp index 2685b3d061ff..01325b5a3f54 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware_obstacle_cruise_planner/planner_interface.hpp @@ -63,12 +63,13 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin, const double enable_approaching_on_curve, - const double additional_safe_distance_margin_on_curve, + const bool use_pointcloud, const double min_behavior_stop_margin, + const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve, const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; + use_pointcloud_ = use_pointcloud; min_behavior_stop_margin_ = min_behavior_stop_margin; enable_approaching_on_curve_ = enable_approaching_on_curve; additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; @@ -117,6 +118,7 @@ class PlannerInterface // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; + bool use_pointcloud_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 3b1f61ca0601..79a8aa4ccb8a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -489,7 +489,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } @@ -547,7 +547,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( min_safe_distance_margin_on_curve_); planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); @@ -888,7 +888,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( vehicle_info_.vehicle_width_m; if (current_lat_dist_from_obstacle_to_traj < max_lat_margin) { - if (!ego_to_obstacle_distance) { + if (!ego_to_obstacle_distance) { const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); @@ -908,12 +908,12 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( lat_dist_from_obstacle_to_traj = std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); } else { - continue; + continue; } } if (ego_to_obstacle_distance) { - target_obstacles.emplace_back( + target_obstacles.emplace_back( pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, slow_down_back_collision_point, *ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); @@ -1532,15 +1532,15 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( object_id.c_str()); debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); return std::nullopt; + } } - } - // calculate collision points with trajectory with lateral stop margin + // calculate collision points with trajectory with lateral stop margin const auto traj_polys_with_lat_margin = createOneStepPolygons( traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); collision_point = polygon_utils::getCollisionPoint( - traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); + traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); } if (!collision_point) { return std::nullopt; @@ -1624,73 +1624,73 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), precise_lat_dist); return std::nullopt; - } + } const auto obstacle_poly = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); - // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not divided by two. - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, - p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); - - std::vector front_collision_polygons; - size_t front_seg_idx = 0; - std::vector back_collision_polygons; - size_t back_seg_idx = 0; - for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) { - std::vector collision_polygons; - bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); - - if (!collision_polygons.empty()) { - if (front_collision_polygons.empty()) { - front_collision_polygons = collision_polygons; - front_seg_idx = i == 0 ? i : i - 1; - } - back_collision_polygons = collision_polygons; - back_seg_idx = i == 0 ? i : i - 1; - } else { - if (!back_collision_polygons.empty()) { - break; // for efficient calculation + // calculate collision points with trajectory with lateral stop margin + // NOTE: For additional margin, hysteresis is not divided by two. + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, odometry.pose.pose, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); + + std::vector front_collision_polygons; + size_t front_seg_idx = 0; + std::vector back_collision_polygons; + size_t back_seg_idx = 0; + for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) { + std::vector collision_polygons; + bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); + + if (!collision_polygons.empty()) { + if (front_collision_polygons.empty()) { + front_collision_polygons = collision_polygons; + front_seg_idx = i == 0 ? i : i - 1; + } + back_collision_polygons = collision_polygons; + back_seg_idx = i == 0 ? i : i - 1; + } else { + if (!back_collision_polygons.empty()) { + break; // for efficient calculation + } } } - } - if (front_collision_polygons.empty() || back_collision_polygons.empty()) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str()); - return std::nullopt; - } + if (front_collision_polygons.empty() || back_collision_polygons.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str()); + return std::nullopt; + } - // calculate front collision point - double front_min_dist = std::numeric_limits::max(); - for (const auto & collision_poly : front_collision_polygons) { - for (const auto & collision_point : collision_poly.outer()) { - const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( - traj_points, front_seg_idx, collision_geom_point); - if (dist < front_min_dist) { - front_min_dist = dist; - front_collision_point = collision_geom_point; + // calculate front collision point + double front_min_dist = std::numeric_limits::max(); + for (const auto & collision_poly : front_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = motion_utils::calcLongitudinalOffsetToSegment( + traj_points, front_seg_idx, collision_geom_point); + if (dist < front_min_dist) { + front_min_dist = dist; + front_collision_point = collision_geom_point; + } } } - } - // calculate back collision point - double back_max_dist = -std::numeric_limits::max(); + // calculate back collision point + double back_max_dist = -std::numeric_limits::max(); back_collision_point = front_collision_point; - for (const auto & collision_poly : back_collision_polygons) { - for (const auto & collision_point : collision_poly.outer()) { - const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( - traj_points, back_seg_idx, collision_geom_point); - if (back_max_dist < dist) { - back_max_dist = dist; - back_collision_point = collision_geom_point; + for (const auto & collision_poly : back_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = motion_utils::calcLongitudinalOffsetToSegment( + traj_points, back_seg_idx, collision_geom_point); + if (back_max_dist < dist) { + back_max_dist = dist; + back_collision_point = collision_geom_point; + } } } - } } if (!front_collision_point || !back_collision_point) { return std::nullopt; diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index f3b5af842acd..146f4479844a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -438,7 +438,7 @@ std::vector PlannerInterface::generateStopTrajectory( double PlannerInterface::calculateMarginFromObstacleOnCurve( const PlannerData & planner_data, const StopObstacle & stop_obstacle) const { - if (!enable_approaching_on_curve_) { + if (!enable_approaching_on_curve_ || use_pointcloud_) { return longitudinal_info_.safe_distance_margin; } From 4925d0b204c9dc4707768a42e797f881d625a523 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 12 Jun 2024 18:00:27 +0900 Subject: [PATCH 37/64] fix obstacle conversion --- .../autoware_obstacle_cruise_planner/src/node.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 79a8aa4ccb8a..3ebbb268feb4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -911,13 +911,13 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } } + } - if (ego_to_obstacle_distance) { - target_obstacles.emplace_back( - pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, - slow_down_back_collision_point, *ego_to_obstacle_distance, - lat_dist_from_obstacle_to_traj); - } + if (ego_to_obstacle_distance) { + target_obstacles.emplace_back( + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, *ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj); } } } @@ -1711,8 +1711,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - const auto & p = behavior_determination_param_; - if (use_pointcloud_) { return; } From fea93ed408b7ff46b5dd4771329cca3030bd0a5b Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 13 Jun 2024 12:25:05 +0900 Subject: [PATCH 38/64] fix collision point determination --- .../src/node.cpp | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 3ebbb268feb4..ab27bebe4515 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -869,9 +869,10 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( kdtree.setInputCloud(cluster_points_ptr); const auto max_lat_margin = - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_slow_down); + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + double lat_dist_from_stop_collision_to_traj_poly = std::numeric_limits::max(); double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); - std::optional ego_to_obstacle_distance = std::nullopt; + double ego_to_obstacle_distance = std::numeric_limits::min(); std::optional stop_collision_point = std::nullopt; std::optional slow_down_front_collision_point = std::nullopt; std::optional slow_down_back_collision_point = std::nullopt; @@ -884,21 +885,27 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( const auto nearest_obstacle_point = toGeomPoint(cluster_points_ptr->points[index.front()]); const auto current_lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, nearest_obstacle_point) - - vehicle_info_.vehicle_width_m; + motion_utils::calcLateralOffset(traj_points, nearest_obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; - if (current_lat_dist_from_obstacle_to_traj < max_lat_margin) { - if (!ego_to_obstacle_distance) { + if (min_lat_dist_to_traj_poly < max_lat_margin) { const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - ego_to_obstacle_distance = + const auto current_ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::max(ego_to_obstacle_distance, current_ego_to_obstacle_distance.value()); + } else { + continue; } - if (current_lat_dist_from_obstacle_to_traj < p.max_lat_margin_for_stop) { - if (!stop_collision_point) { + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (min_lat_dist_to_traj_poly < lat_dist_from_stop_collision_to_traj_poly) { stop_collision_point = nearest_obstacle_point; + lat_dist_from_stop_collision_to_traj_poly = min_lat_dist_to_traj_poly; } - } - if (current_lat_dist_from_obstacle_to_traj < p.max_lat_margin_for_slow_down) { + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { if (!slow_down_front_collision_point) { slow_down_front_collision_point = nearest_obstacle_point; } else { @@ -913,11 +920,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } } - if (ego_to_obstacle_distance) { + if ( + stop_collision_point || slow_down_front_collision_point || slow_down_back_collision_point) { target_obstacles.emplace_back( pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, - slow_down_back_collision_point, *ego_to_obstacle_distance, - lat_dist_from_obstacle_to_traj); + slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); } } } From 12044c15b78673778baefe40217634bc31b3943c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 13 Jun 2024 12:25:53 +0900 Subject: [PATCH 39/64] simplify pointcloud transformation --- .../autoware_obstacle_cruise_planner/src/node.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index ab27bebe4515..e1e8616b1520 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -824,17 +824,16 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } if (transform_stamped && !pointcloud.data.empty()) { - PointCloud2 transformed_points{}; - const Eigen::Matrix4f affine_matrix = + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(pointcloud, *pointcloud_ptr); + const Eigen::Matrix4f transform = tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); - pcl_ros::transformPointCloud(affine_matrix, pointcloud, transformed_points); - PointCloud::Ptr transformed_points_ptr(new PointCloud); - pcl::fromROSMsg(transformed_points, *transformed_points_ptr); + pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); // 2. downsample & cluster pointcloud PointCloud::Ptr filtered_points_ptr(new PointCloud); pcl::VoxelGrid filter; - filter.setInputCloud(transformed_points_ptr); + filter.setInputCloud(pointcloud_ptr); filter.setLeafSize( p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); filter.filter(*filtered_points_ptr); From 39d7a30800e69a8f19b23f7d9b71b0d298a35c78 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Jun 2024 03:28:23 +0000 Subject: [PATCH 40/64] style(pre-commit): autofix --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index e1e8616b1520..ec286ced9ecd 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -889,9 +889,9 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; if (min_lat_dist_to_traj_poly < max_lat_margin) { - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto current_ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); + calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); if (current_ego_to_obstacle_distance) { ego_to_obstacle_distance = std::max(ego_to_obstacle_distance, current_ego_to_obstacle_distance.value()); From 4c3519dcb07640fb7f898602cb1cfc34633c2144 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 13 Jun 2024 13:08:34 +0900 Subject: [PATCH 41/64] fix collision point determination --- .../src/node.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index ec286ced9ecd..4cd58086584b 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -869,9 +869,13 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( const auto max_lat_margin = std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + double lat_dist_from_stop_collision_to_traj_poly = std::numeric_limits::max(); + double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); - double ego_to_obstacle_distance = std::numeric_limits::min(); + double ego_to_obstacle_distance = std::numeric_limits::max(); std::optional stop_collision_point = std::nullopt; std::optional slow_down_front_collision_point = std::nullopt; std::optional slow_down_back_collision_point = std::nullopt; @@ -889,12 +893,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; if (min_lat_dist_to_traj_poly < max_lat_margin) { - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto current_ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); if (current_ego_to_obstacle_distance) { ego_to_obstacle_distance = - std::max(ego_to_obstacle_distance, current_ego_to_obstacle_distance.value()); + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); } else { continue; } @@ -905,10 +908,13 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( lat_dist_from_stop_collision_to_traj_poly = min_lat_dist_to_traj_poly; } } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { - if (!slow_down_front_collision_point) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { slow_down_front_collision_point = nearest_obstacle_point; - } else { + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { slow_down_back_collision_point = nearest_obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; } } lat_dist_from_obstacle_to_traj = From d7851ff23efb5904df867055f0f998cd1202b649 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 13 Jun 2024 15:37:41 +0900 Subject: [PATCH 42/64] pick nearest stop collision point --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 4cd58086584b..0def7a1b7b37 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -871,7 +871,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - double lat_dist_from_stop_collision_to_traj_poly = std::numeric_limits::max(); + double ego_to_stop_collision_distance = std::numeric_limits::max(); double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); @@ -903,9 +903,9 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { - if (min_lat_dist_to_traj_poly < lat_dist_from_stop_collision_to_traj_poly) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { stop_collision_point = nearest_obstacle_point; - lat_dist_from_stop_collision_to_traj_poly = min_lat_dist_to_traj_poly; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; } } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { @@ -925,8 +925,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } } - if ( - stop_collision_point || slow_down_front_collision_point || slow_down_back_collision_point) { + if (stop_collision_point || slow_down_front_collision_point) { target_obstacles.emplace_back( pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); From cc2951017c50dbf36ce59cf08301bee33fdca3bf Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 13 Jun 2024 17:05:23 +0900 Subject: [PATCH 43/64] check collision for every point in cluster --- .../src/node.cpp | 113 ++++++------------ 1 file changed, 35 insertions(+), 78 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 0def7a1b7b37..4a85587422cb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -28,11 +28,7 @@ #include -#include -#include #include -#include -#include #include #include @@ -65,22 +61,6 @@ std::optional getObstacleFromUuid( return *itr; } -geometry_msgs::msg::Pose getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info) -{ - const auto & i = vehicle_info; - const auto yaw = tier4_autoware_utils::getRPY(base_pose).z; - - geometry_msgs::msg::Pose center_pose; - center_pose.position.x = - base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); - center_pose.position.y = - base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); - center_pose.position.z = base_pose.position.z; - center_pose.orientation = base_pose.orientation; - return center_pose; -} - std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) @@ -849,28 +829,12 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( ec.setInputCloud(filtered_points_ptr); ec.extract(clusters); - std::vector center_points(traj_points.size()); - for (const auto & traj_point : traj_points) { - center_points.push_back(getVehicleCenterFromBase(traj_point.pose, vehicle_info_).position); - } + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); // 3. convert clusters to obstacles for (const auto & cluster_indices : clusters) { - pcl::IndicesConstPtr cluster_indices_ptr = - pcl::make_shared(cluster_indices.indices); - PointCloud::Ptr cluster_points_ptr(new PointCloud); - pcl::ExtractIndices extract; - extract.setInputCloud(filtered_points_ptr); - extract.setIndices(cluster_indices_ptr); - extract.filter(*cluster_points_ptr); - - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cluster_points_ptr); - - const auto max_lat_margin = - std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - double ego_to_stop_collision_distance = std::numeric_limits::max(); double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); @@ -880,48 +844,41 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( std::optional slow_down_front_collision_point = std::nullopt; std::optional slow_down_back_collision_point = std::nullopt; - for (const auto & center_point : center_points) { - pcl::PointXYZ point(center_point.x, center_point.y, 0.0); - pcl::Indices index(1); - std::vector sqr_distance(1); - if (kdtree.nearestKSearch(point, 1, index, sqr_distance) > 0) { - const auto nearest_obstacle_point = - toGeomPoint(cluster_points_ptr->points[index.front()]); - const auto current_lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, nearest_obstacle_point); - const auto min_lat_dist_to_traj_poly = - std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; - - if (min_lat_dist_to_traj_poly < max_lat_margin) { - const auto current_ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, nearest_obstacle_point); - if (current_ego_to_obstacle_distance) { - ego_to_obstacle_distance = - std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); - } else { - continue; - } - - if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { - if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { - stop_collision_point = nearest_obstacle_point; - ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; - } - } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { - if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { - slow_down_front_collision_point = nearest_obstacle_point; - ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; - } else if ( - *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { - slow_down_back_collision_point = nearest_obstacle_point; - ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; - } - } - lat_dist_from_obstacle_to_traj = - std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + for (const auto & index : cluster_indices.indices) { + const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); + const auto current_lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < max_lat_margin) { + const auto current_ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); } else { continue; } + + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { + stop_collision_point = obstacle_point; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + } + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { + slow_down_front_collision_point = obstacle_point; + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { + slow_down_back_collision_point = obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; + } + } } } From 63f6e1e21f61631c80368d097d88e7c7ab8385e5 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Wed, 19 Jun 2024 12:01:11 +0900 Subject: [PATCH 44/64] migrate previous changes to new files --- .../common_structs.hpp | 32 +- .../autoware/obstacle_cruise_planner/node.hpp | 28 +- .../obstacle_cruise_planner/type_alias.hpp | 7 + .../src/node.cpp | 501 +++++++++--------- 4 files changed, 313 insertions(+), 255 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index a7f4787f76c4..0a46d1b787ac 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -58,15 +58,15 @@ struct Obstacle const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), uuid(autoware_universe_utils::toHexString(object.object_id)), - shape(object.shape), - ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) + shape(object.shape) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -74,9 +74,26 @@ struct Obstacle } } - Polygon2d toPolygon() const { return autoware_universe_utils::toPolygon2d(pose, shape); } + Obstacle( + const rclcpp::Time & arg_stamp, + const std::optional & stop_collision_point, + const std::optional & slow_down_front_collision_point, + const std::optional & slow_down_back_collision_point, + const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) + : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + stop_collision_point(stop_collision_point), + slow_down_front_collision_point(slow_down_front_collision_point), + slow_down_back_collision_point(slow_down_back_collision_point) + { + } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. + double ego_to_obstacle_distance; + double lat_dist_from_obstacle_to_traj; + + // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp bool orientation_reliable; Twist twist; @@ -85,8 +102,11 @@ struct Obstacle std::string uuid; Shape shape; std::vector predicted_paths; - double ego_to_obstacle_distance; - double lat_dist_from_obstacle_to_traj; + + // for PointCloud + std::optional stop_collision_point; + std::optional slow_down_front_collision_point; + std::optional slow_down_back_collision_point; }; struct TargetObstacleInterface diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 5d0d99a742d1..e5c66262f4f2 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -27,6 +27,9 @@ #include #include +#include +#include + #include #include #include @@ -58,10 +61,18 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, + const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PointCloud2 & /* pointcloud */, + const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; std::optional createStopObstacle( @@ -116,6 +127,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; + bool use_pointcloud_{false}; + bool enable_slow_down_planning_{false}; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; double additional_safe_distance_margin_on_curve_; @@ -126,6 +139,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -149,9 +164,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/odometry"}; autoware_universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; + autoware_universe_utils::InterProcessPollingSubscriber pointcloud_sub_{ + this, "~/input/pointcloud"}; autoware_universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + // Vehicle Parameters VehicleInfo vehicle_info_; @@ -182,6 +202,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onParam(const std::vector & parameters); double decimate_trajectory_step_length; + double pointcloud_search_radius; + double pointcloud_voxel_grid_x; + double pointcloud_voxel_grid_y; + double pointcloud_voxel_grid_z; + double pointcloud_cluster_tolerance; + int pointcloud_min_cluster_size; + int pointcloud_max_cluster_size; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; @@ -270,7 +297,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; - bool enable_slow_down_planning_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index bc9f6070e159..81f2be078c82 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -27,6 +27,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_planning_msgs/msg/stop_factor.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" @@ -35,6 +36,9 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include + using autoware::vehicle_info_utils::VehicleInfo; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; @@ -50,6 +54,7 @@ using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -63,4 +68,6 @@ namespace bg = boost::geometry; using autoware_universe_utils::Point2d; using autoware_universe_utils::Polygon2d; +using PointCloud = pcl::PointCloud; + #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 7b8b8f32534a..8a74a5a48989 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -852,7 +852,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( for (const auto & index : cluster_indices.indices) { const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); const auto current_lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, obstacle_point); + autoware_motion_utils::calcLateralOffset(traj_points, obstacle_point); const auto min_lat_dist_to_traj_poly = std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; @@ -966,7 +966,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware_universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); @@ -1448,8 +1448,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::optional> collision_point = std::nullopt; if (use_pointcloud_) { if (obstacle.stop_collision_point) { - const auto dist_to_collide_on_traj = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.stop_collision_point.value()); + const auto dist_to_collide_on_traj = autoware_motion_utils::calcSignedArcLength( + traj_points, 0, obstacle.stop_collision_point.value()); collision_point = std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); } @@ -1600,7 +1600,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } - const auto obstacle_poly = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware_universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. @@ -1639,11 +1639,10 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate front collision point double front_min_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point front_collision_point; for (const auto & collision_poly : front_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, front_seg_idx, collision_geom_point); if (dist < front_min_dist) { front_min_dist = dist; @@ -1654,11 +1653,11 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate back collision point double back_max_dist = -std::numeric_limits::max(); - geometry_msgs::msg::Point back_collision_point = front_collision_point; + back_collision_point = front_collision_point; for (const auto & collision_poly : back_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, back_seg_idx, collision_geom_point); if (back_max_dist < dist) { back_max_dist = dist; @@ -1666,282 +1665,288 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } } } - - const auto [tangent_vel, normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, tangent_vel, normal_vel, - precise_lat_dist, front_collision_point, back_collision_point}; + } + if (!front_collision_point || !back_collision_point) { + return std::nullopt; } - void ObstacleCruisePlannerNode::checkConsistency( - const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, - std::vector & stop_obstacles) - { + const auto [tangent_vel, normal_vel] = [&]() { if (use_pointcloud_) { - return; + return std::make_pair(0., 0.); } + return projectObstacleVelocityToTrajectory(traj_points, obstacle); + }(); + return SlowDownObstacle{ + obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, + normal_vel, precise_lat_dist, *front_collision_point, *back_collision_point}; +} - for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { - const auto predicted_object_itr = std::find_if( - predicted_objects.objects.begin(), predicted_objects.objects.end(), - [&prev_closest_stop_obstacle](const PredictedObject & po) { - return autoware_universe_utils::toHexString(po.object_id) == - prev_closest_stop_obstacle.uuid; - }); - // If previous closest obstacle disappear from the perception result, do nothing anymore. - if (predicted_object_itr == predicted_objects.objects.end()) { - continue; - } +void ObstacleCruisePlannerNode::checkConsistency( + const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, + std::vector & stop_obstacles) +{ + if (use_pointcloud_) { + return; + } - const auto is_disappeared_from_stop_obstacle = std::none_of( - stop_obstacles.begin(), stop_obstacles.end(), - [&prev_closest_stop_obstacle](const StopObstacle & so) { - return so.uuid == prev_closest_stop_obstacle.uuid; - }); - if (is_disappeared_from_stop_obstacle) { - // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" - // condition is satisfied - const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); - if ( - predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < - behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && - elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { - stop_obstacles.push_back(prev_closest_stop_obstacle); - } - } + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto predicted_object_itr = std::find_if( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [&prev_closest_stop_obstacle](const PredictedObject & po) { + return autoware_universe_utils::toHexString(po.object_id) == + prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (predicted_object_itr == predicted_objects.objects.end()) { + continue; } - prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && + elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } + } } - bool ObstacleCruisePlannerNode::isObstacleCrossing( - const std::vector & traj_points, const Obstacle & obstacle) const - { - const double diff_angle = calcDiffAngleAgainstTrajectory(traj_points, obstacle.pose); - - // NOTE: Currently predicted objects does not have orientation availability even - // though sometimes orientation is not available. - const bool is_obstacle_crossing_trajectory = - behavior_determination_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) && - behavior_determination_param_.crossing_obstacle_traj_angle_threshold < - M_PI - std::abs(diff_angle); - if (!is_obstacle_crossing_trajectory) { - return false; - } + prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); +} - // Only obstacles crossing the ego's trajectory with high speed are considered. - return true; +bool ObstacleCruisePlannerNode::isObstacleCrossing( + const std::vector & traj_points, const Obstacle & obstacle) const +{ + const double diff_angle = calcDiffAngleAgainstTrajectory(traj_points, obstacle.pose); + + // NOTE: Currently predicted objects does not have orientation availability even + // though sometimes orientation is not available. + const bool is_obstacle_crossing_trajectory = + behavior_determination_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) && + behavior_determination_param_.crossing_obstacle_traj_angle_threshold < + M_PI - std::abs(diff_angle); + if (!is_obstacle_crossing_trajectory) { + return false; } - double ObstacleCruisePlannerNode::calcCollisionTimeMargin( - const Odometry & odometry, const std::vector & collision_points, - const std::vector & traj_points, const bool is_driving_forward) const - { - const auto & ego_pose = odometry.pose.pose; - const double ego_vel = odometry.twist.twist.linear.x; - - const double time_to_reach_collision_point = [&]() { - const double abs_ego_offset = is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_from_ego_to_obstacle = - std::abs(autoware_motion_utils::calcSignedArcLength( - traj_points, ego_pose.position, collision_points.front().point)) - - abs_ego_offset; - return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); - }(); + // Only obstacles crossing the ego's trajectory with high speed are considered. + return true; +} - const double time_to_start_cross = - (rclcpp::Time(collision_points.front().stamp) - now()).seconds(); - const double time_to_end_cross = - (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); +double ObstacleCruisePlannerNode::calcCollisionTimeMargin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const bool is_driving_forward) const +{ + const auto & ego_pose = odometry.pose.pose; + const double ego_vel = odometry.twist.twist.linear.x; + + const double time_to_reach_collision_point = [&]() { + const double abs_ego_offset = is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double dist_from_ego_to_obstacle = + std::abs(autoware_motion_utils::calcSignedArcLength( + traj_points, ego_pose.position, collision_points.front().point)) - + abs_ego_offset; + return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); + }(); - if (time_to_reach_collision_point < time_to_start_cross) { // Ego goes first. - return time_to_start_cross - time_to_reach_collision_point; - } - if (time_to_end_cross < time_to_reach_collision_point) { // Obstacle goes first. - return time_to_reach_collision_point - time_to_end_cross; - } - return 0.0; // Ego and obstacle will collide. - } + const double time_to_start_cross = + (rclcpp::Time(collision_points.front().stamp) - now()).seconds(); + const double time_to_end_cross = (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); - PlannerData ObstacleCruisePlannerNode::createPlannerData( - const Odometry & odometry, const AccelWithCovarianceStamped & acc, - const std::vector & traj_points) const - { - PlannerData planner_data; - planner_data.current_time = now(); - planner_data.traj_points = traj_points; - planner_data.ego_pose = odometry.pose.pose; - planner_data.ego_vel = odometry.twist.twist.linear.x; - planner_data.ego_acc = acc.accel.accel.linear.x; - planner_data.is_driving_forward = is_driving_forward_; - return planner_data; + if (time_to_reach_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_collision_point; } + if (time_to_end_cross < time_to_reach_collision_point) { // Obstacle goes first. + return time_to_reach_collision_point - time_to_end_cross; + } + return 0.0; // Ego and obstacle will collide. +} - void ObstacleCruisePlannerNode::publishVelocityLimit( - const std::optional & vel_limit, const std::string & module_name) - { - if (vel_limit) { - vel_limit_pub_->publish(*vel_limit); - need_to_clear_vel_limit_.at(module_name) = true; - return; - } +PlannerData ObstacleCruisePlannerNode::createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, + const std::vector & traj_points) const +{ + PlannerData planner_data; + planner_data.current_time = now(); + planner_data.traj_points = traj_points; + planner_data.ego_pose = odometry.pose.pose; + planner_data.ego_vel = odometry.twist.twist.linear.x; + planner_data.ego_acc = acc.accel.accel.linear.x; + planner_data.is_driving_forward = is_driving_forward_; + return planner_data; +} - if (!need_to_clear_vel_limit_.at(module_name)) { - return; - } +void ObstacleCruisePlannerNode::publishVelocityLimit( + const std::optional & vel_limit, const std::string & module_name) +{ + if (vel_limit) { + vel_limit_pub_->publish(*vel_limit); + need_to_clear_vel_limit_.at(module_name) = true; + return; + } - // clear velocity limit - const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now(), module_name); - clear_vel_limit_pub_->publish(clear_vel_limit_msg); - need_to_clear_vel_limit_.at(module_name) = false; + if (!need_to_clear_vel_limit_.at(module_name)) { + return; } - void ObstacleCruisePlannerNode::publishDebugMarker() const - { - stop_watch_.tic(__func__); - - // 1. publish debug marker - MarkerArray debug_marker; - - // obstacles to cruise - std::vector stop_collision_points; - for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { - // obstacle - const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.6, 0.1); - debug_marker.markers.push_back(obstacle_marker); - - // collision points - for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size(); - ++j) { - stop_collision_points.push_back( - debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); - } - } - for (size_t i = 0; i < stop_collision_points.size(); ++i) { - auto collision_point_marker = autoware_universe_utils::createDefaultMarker( - "map", now(), "cruise_collision_points", i, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); - collision_point_marker.pose.position = stop_collision_points.at(i); - debug_marker.markers.push_back(collision_point_marker); - } + // clear velocity limit + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now(), module_name); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_.at(module_name) = false; +} - // obstacles to stop - for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) { - // obstacle - const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles_to_stop", 1.0, 0.0, 0.0); - debug_marker.markers.push_back(obstacle_marker); - - // collision point - auto collision_point_marker = autoware_universe_utils::createDefaultMarker( - "map", now(), "stop_collision_points", 0, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); - collision_point_marker.pose.position = - debug_data_ptr_->obstacles_to_stop.at(i).collision_point; - debug_marker.markers.push_back(collision_point_marker); - } +void ObstacleCruisePlannerNode::publishDebugMarker() const +{ + stop_watch_.tic(__func__); - // obstacles to slow down - for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) { - // obstacle - const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles_to_slow_down", 0.7, 0.7, - 0.0); - debug_marker.markers.push_back(obstacle_marker); - - // collision points - auto front_collision_point_marker = autoware_universe_utils::createDefaultMarker( - "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); - front_collision_point_marker.pose.position = - debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; - auto back_collision_point_marker = autoware_universe_utils::createDefaultMarker( - "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, - autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), - autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); - back_collision_point_marker.pose.position = - debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; - - debug_marker.markers.push_back(front_collision_point_marker); - debug_marker.markers.push_back(back_collision_point_marker); + // 1. publish debug marker + MarkerArray debug_marker; + + // obstacles to cruise + std::vector stop_collision_points; + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.6, 0.1); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size(); + ++j) { + stop_collision_points.push_back( + debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); } + } + for (size_t i = 0; i < stop_collision_points.size(); ++i) { + auto collision_point_marker = autoware_universe_utils::createDefaultMarker( + "map", now(), "cruise_collision_points", i, Marker::SPHERE, + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = stop_collision_points.at(i); + debug_marker.markers.push_back(collision_point_marker); + } - // intentionally ignored obstacles to cruise or stop - for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { - const auto marker = obstacle_cruise_utils::getObjectMarker( - debug_data_ptr_->intentionally_ignored_obstacles.at(i).pose, i, - "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); - debug_marker.markers.push_back(marker); - } + // obstacles to stop + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles_to_stop", 1.0, 0.0, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision point + auto collision_point_marker = autoware_universe_utils::createDefaultMarker( + "map", now(), "stop_collision_points", 0, Marker::SPHERE, + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; + debug_marker.markers.push_back(collision_point_marker); + } - { // footprint polygons - auto marker = autoware_universe_utils::createDefaultMarker( - "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), - autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); - - for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { - for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { - const auto & current_point = detection_polygon.outer().at(dp_idx); - const auto & next_point = - detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); - - marker.points.push_back( - autoware_universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); - marker.points.push_back( - autoware_universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); - } + // obstacles to slow down + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles_to_slow_down", 0.7, 0.7, + 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + auto front_collision_point_marker = autoware_universe_utils::createDefaultMarker( + "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + front_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; + auto back_collision_point_marker = autoware_universe_utils::createDefaultMarker( + "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, + autoware_universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware_universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + back_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; + + debug_marker.markers.push_back(front_collision_point_marker); + debug_marker.markers.push_back(back_collision_point_marker); + } + + // intentionally ignored obstacles to cruise or stop + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i).pose, i, + "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + { // footprint polygons + auto marker = autoware_universe_utils::createDefaultMarker( + "map", now(), "detection_polygons", 0, Marker::LINE_LIST, + autoware_universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware_universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { + for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { + const auto & current_point = detection_polygon.outer().at(dp_idx); + const auto & next_point = + detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); + + marker.points.push_back( + autoware_universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + autoware_universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); } - debug_marker.markers.push_back(marker); } + debug_marker.markers.push_back(marker); + } - // slow down debug wall marker - autoware_universe_utils::appendMarkerArray( - debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + // slow down debug wall marker + autoware_universe_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); - debug_marker_pub_->publish(debug_marker); + debug_marker_pub_->publish(debug_marker); - // 2. publish virtual wall for cruise and stop - debug_cruise_wall_marker_pub_->publish(debug_data_ptr_->cruise_wall_marker); - debug_stop_wall_marker_pub_->publish(debug_data_ptr_->stop_wall_marker); - debug_slow_down_wall_marker_pub_->publish(debug_data_ptr_->slow_down_wall_marker); + // 2. publish virtual wall for cruise and stop + debug_cruise_wall_marker_pub_->publish(debug_data_ptr_->cruise_wall_marker); + debug_stop_wall_marker_pub_->publish(debug_data_ptr_->stop_wall_marker); + debug_slow_down_wall_marker_pub_->publish(debug_data_ptr_->slow_down_wall_marker); - // 3. print calculation time - const double calculation_time = stop_watch_.toc(__func__); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", - __func__, calculation_time); - } + // 3. print calculation time + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); +} - void ObstacleCruisePlannerNode::publishDebugInfo() const - { - // stop - const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); - debug_stop_planning_info_pub_->publish(stop_debug_msg); +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); + debug_stop_planning_info_pub_->publish(stop_debug_msg); - // cruise - const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); - debug_cruise_planning_info_pub_->publish(cruise_debug_msg); + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); - // slow_down - const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); - debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); - } + // slow_down + const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); + debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); +} - void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const - { - Float32Stamped calculation_time_msg; - calculation_time_msg.stamp = now(); - calculation_time_msg.data = calculation_time; - debug_calculation_time_pub_->publish(calculation_time_msg); - } +void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const +{ + Float32Stamped calculation_time_msg; + calculation_time_msg.stamp = now(); + calculation_time_msg.data = calculation_time; + debug_calculation_time_pub_->publish(calculation_time_msg); +} } // namespace autoware::motion_planning #include From 0c913b239d1ee9f54a2374c6e0b2c5a42fac78cf Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 13:35:01 +0900 Subject: [PATCH 45/64] reduce diff --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 8a74a5a48989..2a1a3ecbbf39 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -1109,12 +1109,12 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) { - const auto & p = behavior_determination_param_; - if (use_pointcloud_) { return std::nullopt; } + const auto & p = behavior_determination_param_; + const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: When driving backward, Stop will be planned instead of cruise. @@ -1157,12 +1157,12 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points) { - const auto & p = behavior_determination_param_; - if (use_pointcloud_) { return std::nullopt; } + const auto & p = behavior_determination_param_; + if (traj_points.empty()) return std::nullopt; // check label const auto & object_id = obstacle.uuid.substr(0, 4); From f2dc8cf5dc8e8521abf7009124b33171a52a9533 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 14:05:47 +0900 Subject: [PATCH 46/64] remove use_pointcloud parameter --- .../include/autoware/obstacle_cruise_planner/node.hpp | 6 +++--- .../autoware_obstacle_cruise_planner/src/node.cpp | 11 +++++------ 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index e5c66262f4f2..e31fd40277de 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -127,8 +127,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; - bool use_pointcloud_{false}; bool enable_slow_down_planning_{false}; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; double additional_safe_distance_margin_on_curve_; @@ -139,8 +140,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; - bool use_pointcloud_for_stop_; - bool use_pointcloud_for_slow_down_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -297,6 +296,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; + bool use_pointcloud_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 2a1a3ecbbf39..2d27dfc3bb3e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -442,8 +442,12 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & enable_debug_info_ = declare_parameter("common.enable_debug_info"); enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info"); - use_pointcloud_ = declare_parameter("common.use_pointcloud"); enable_slow_down_planning_ = declare_parameter("common.enable_slow_down_planning"); + + use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = + declare_parameter("common.slow_down_obstacle_type.pointcloud"); + use_pointcloud_ = use_pointcloud_for_stop_ || use_pointcloud_for_slow_down_; behavior_determination_param_ = BehaviorDeterminationParam(*this); @@ -484,9 +488,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & outside_cruise_obstacle_types_ = getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); - use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); - use_pointcloud_for_slow_down_ = - declare_parameter("common.slow_down_obstacle_type.pointcloud"); } // set parameter callback @@ -519,8 +520,6 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( autoware_universe_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); - autoware_universe_utils::updateParam(parameters, "common.use_pointcloud", use_pointcloud_); - autoware_universe_utils::updateParam( parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); autoware_universe_utils::updateParam( From bb2c5d62bbdb1aef6db596ff697e99d8d3f85b07 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 14:06:34 +0900 Subject: [PATCH 47/64] add parameters for pointcloud filtering --- .../config/obstacle_cruise_planner.param.yaml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 4000227fd5dc..e797faaff9aa 100644 --- a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -72,7 +72,14 @@ pointcloud: false behavior_determination: - use_pointcloud: false + pointcloud_search_radius: 5.0 + pointcloud_voxel_grid_x: 0.05 + pointcloud_voxel_grid_y: 0.05 + pointcloud_voxel_grid_z: 100000.0 + pointcloud_cluster_tolerance: 1.0 + pointcloud_min_cluster_size: 1 + pointcloud_max_cluster_size: 100000 + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking prediction_resampling_time_interval: 0.1 From 299824b95ca89442e9633820b0efdff9dfdc2ad3 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 14:28:36 +0900 Subject: [PATCH 48/64] add autoware namespace --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 60fb210daa55..75e2b3d25086 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -851,7 +851,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( for (const auto & index : cluster_indices.indices) { const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); const auto current_lat_dist_from_obstacle_to_traj = - autoware_motion_utils::calcLateralOffset(traj_points, obstacle_point); + autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); const auto min_lat_dist_to_traj_poly = std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; @@ -965,7 +965,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = autoware_universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); @@ -1447,7 +1447,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::optional> collision_point = std::nullopt; if (use_pointcloud_) { if (obstacle.stop_collision_point) { - const auto dist_to_collide_on_traj = autoware_motion_utils::calcSignedArcLength( + const auto dist_to_collide_on_traj = autoware::motion_utils::calcSignedArcLength( traj_points, 0, obstacle.stop_collision_point.value()); collision_point = std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); @@ -1599,7 +1599,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } - const auto obstacle_poly = autoware_universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. From 64a3191c82fe3cee6b167b0001f5b892c6a68110 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 15:34:29 +0900 Subject: [PATCH 49/64] Revert "add max_num_points parameter to dummy object" This reverts commit 98bcd0856f861d23c9f7989d8128939ec0b3e27c. --- .../include/dummy_perception_publisher/node.hpp | 1 - simulator/dummy_perception_publisher/msg/Object.msg | 1 - simulator/dummy_perception_publisher/src/node.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 4e1100fec844..c3f7976d3efa 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -53,7 +53,6 @@ struct ObjectInfo double std_dev_y; double std_dev_z; double std_dev_yaw; - std::size_t max_num_points; tf2::Transform tf_map2moved_object; // pose and twist geometry_msgs::msg::TwistWithCovariance twist_covariance_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 7fd79f7f7ae8..11ac1b3d39da 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/simulator/dummy_perception_publisher/msg/Object.msg @@ -5,7 +5,6 @@ autoware_perception_msgs/ObjectClassification classification autoware_perception_msgs/Shape shape float32 max_velocity float32 min_velocity -uint32 max_num_points uint8 ADD=0 uint8 MODIFY=1 diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 6cf07e6f5a4e..b479d7c36bb4 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -45,7 +45,6 @@ ObjectInfo::ObjectInfo( std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])), std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])), std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])), - max_num_points(object.max_num_points), twist_covariance_(object.initial_state.twist_covariance), pose_covariance_(object.initial_state.pose_covariance) { From d5260f751db42978a6eb193a066371857c59c27d Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 15:34:47 +0900 Subject: [PATCH 50/64] Revert "downsample pointcloud when the number of points is larger than max_num_points" This reverts commit fb00b59d8f14cec6810e7fab12bc34d8a0c617c7. --- .../src/pointcloud_creator.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 8353bc1b3cc7..05354fa0a966 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -17,7 +17,6 @@ #include -#include #include #include #include @@ -254,18 +253,6 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr } } - for (size_t i = 0; i < pointclouds.size(); ++i) { - auto & cloud = pointclouds.at(i); - const auto max_num_points = obj_infos.at(i).max_num_points; - if (max_num_points != 0 && cloud->size() > max_num_points) { - pcl::Indices indices; - pcl::ExtractIndices filter; - filter.setInputCloud(cloud); - filter.setIndices(0, cloud->size() / 2 - max_num_points / 2, 1, max_num_points); - filter.filter(*cloud); - } - } - for (const auto & cloud : pointclouds) { for (const auto & pt : *cloud) { merged_pointcloud->push_back(pt); From d8c8dbb9d4e905c45ed2577f8e422a3bdd72b7e3 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 20 Jun 2024 15:34:55 +0900 Subject: [PATCH 51/64] Revert "add max_num_points property to dummy bus" This reverts commit 5f9e4ab5ae7d8d46521c736b1d259040121f3bc5. --- common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp | 4 ---- .../src/tools/interactive_object.hpp | 2 -- 2 files changed, 6 deletions(-) diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp index 3e66935ac7a2..49da06aa2478 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -189,8 +189,6 @@ BusInitialPoseTool::BusInitialPoseTool() "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer()); min_velocity_ = new rviz_common::properties::FloatProperty( "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer()); - max_num_points_ = new rviz_common::properties::IntProperty( - "Max number of points", 0, "Max number of points in point cloud", getPropertyContainer()); std_dev_x_->setMin(0); std_dev_y_->setMin(0); std_dev_z_->setMin(0); @@ -237,8 +235,6 @@ Object BusInitialPoseTool::createObjectMsg() const object.initial_state.pose_covariance.covariance[35] = std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - object.max_num_points = max_num_points_->getInt(); - std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index 532092631444..c1ec65a0488b 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include #include @@ -164,7 +163,6 @@ protected Q_SLOTS: rviz_common::properties::FloatProperty * max_velocity_; rviz_common::properties::FloatProperty * min_velocity_; rviz_common::properties::FloatProperty * accel_; - rviz_common::properties::IntProperty * max_num_points_; rviz_common::properties::TfFrameProperty * property_frame_; private: From 5f3132f01b0c73a7c87c4b7ebffb69abc9273cea Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 19 Jun 2024 22:13:45 +0900 Subject: [PATCH 52/64] feat(diagnostic_graph_utils): add logging tool Signed-off-by: Takagi, Isamu --- system/diagnostic_graph_utils/CMakeLists.txt | 8 +- .../launch/logging.launch.xml | 10 ++ .../src/node/logging.cpp | 95 +++++++++++++++++++ .../src/node/logging.hpp | 48 ++++++++++ 4 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 system/diagnostic_graph_utils/launch/logging.launch.xml create mode 100644 system/diagnostic_graph_utils/src/node/logging.cpp create mode 100644 system/diagnostic_graph_utils/src/node/logging.hpp diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index b68e7bedb57e..0c36964f4923 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp src/node/converter.cpp + src/node/logging.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_tools @@ -24,4 +25,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools EXECUTABLE converter_node ) -ament_auto_package() +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::LoggingNode" + EXECUTABLE logging_node +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/diagnostic_graph_utils/launch/logging.launch.xml b/system/diagnostic_graph_utils/launch/logging.launch.xml new file mode 100644 index 000000000000..12636574eb8b --- /dev/null +++ b/system/diagnostic_graph_utils/launch/logging.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp new file mode 100644 index 000000000000..86dd84b5152b --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/logging.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "logging.hpp" + +#include +#include +#include +#include +#include +#include + +namespace diagnostic_graph_utils +{ + +LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", options) +{ + root_path_ = declare_parameter("root_path"); + max_depth_ = declare_parameter("max_depth"); + + using std::placeholders::_1; + sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1)); + sub_graph_.subscribe(*this, 1); + + const auto period = rclcpp::Rate(declare_parameter("show_rate")).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); }); +} + +void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + // Search root node. + root_unit_ = nullptr; + for (const auto & unit : graph->units()) { + if (unit->path() == root_path_) { + root_unit_ = unit; + return; + } + } + RCLCPP_ERROR_STREAM(get_logger(), "root unit is not found: " << root_path_); +} + +void LoggingNode::on_timer() +{ + static const auto message = "The target mode is not available for the following reasons:"; + if (root_unit_) { + dump_text_.str(""); + dump_text_.clear(std::stringstream::goodbit); + dump_unit(root_unit_, 0, " "); + RCLCPP_WARN_STREAM(get_logger(), message << std::endl << dump_text_.str()); + } +} + +void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & indent) +{ + const auto text_level = [](DiagUnit::DiagnosticLevel level) { + if (level == DiagUnit::DiagnosticStatus::OK) return "OK "; + if (level == DiagUnit::DiagnosticStatus::WARN) return "WARN "; + if (level == DiagUnit::DiagnosticStatus::ERROR) return "ERROR"; + if (level == DiagUnit::DiagnosticStatus::STALE) return "STALE"; + return "-----"; + }; + + if (max_depth_ < depth) { + return; + } + if (unit->level() == DiagUnit::DiagnosticStatus::OK) { + return; + } + + std::string path = unit->path(); + if (path.empty()) { + path = "[anonymous group]"; + } + + dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl; + for (const auto & child : unit->children()) { + dump_unit(child.unit, depth + 1, indent + " "); + } +} + +} // namespace diagnostic_graph_utils + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::LoggingNode) diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp new file mode 100644 index 000000000000..b2d305c81ca9 --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/logging.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 NODE__LOGGING_HPP_ +#define NODE__LOGGING_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace diagnostic_graph_utils +{ + +class LoggingNode : public rclcpp::Node +{ +public: + explicit LoggingNode(const rclcpp::NodeOptions & options); + +private: + void on_create(DiagGraph::ConstSharedPtr graph); + void on_timer(); + void dump_unit(DiagUnit * unit, int depth, const std::string & indent); + DiagGraphSubscription sub_graph_; + rclcpp::TimerBase::SharedPtr timer_; + + DiagUnit * root_unit_; + int max_depth_; + std::string root_path_; + std::ostringstream dump_text_; +}; + +} // namespace diagnostic_graph_utils + +#endif // NODE__LOGGING_HPP_ From 1d946a5bf9e05e6c9003548ee6f527a96d17eded Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 19 Jun 2024 22:23:10 +0900 Subject: [PATCH 53/64] fix all OK Signed-off-by: Takagi, Isamu --- system/diagnostic_graph_utils/src/node/logging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp index 86dd84b5152b..009af51e949c 100644 --- a/system/diagnostic_graph_utils/src/node/logging.cpp +++ b/system/diagnostic_graph_utils/src/node/logging.cpp @@ -53,7 +53,7 @@ void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph) void LoggingNode::on_timer() { static const auto message = "The target mode is not available for the following reasons:"; - if (root_unit_) { + if (root_unit_ && root_unit_->level() != DiagUnit::DiagnosticStatus::OK) { dump_text_.str(""); dump_text_.clear(std::stringstream::goodbit); dump_unit(root_unit_, 0, " "); From 462f624a0de4cbe6342dae33312e305fd4ca653b Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 19 Jun 2024 18:38:25 +0900 Subject: [PATCH 54/64] feat(default_ad_api): add log when operation mode change fails Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/operation_mode.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index d4b430ecd057..510788dd09d0 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -66,8 +66,13 @@ void OperationModeNode::change_mode( const ResponseT res, const OperationModeRequest::_mode_type mode) { if (!mode_available_[mode]) { + RCLCPP_WARN( + get_logger(), + "The target mode is not available. Please check the cause with " + "rqt_diagnostics_graph_monitor"); throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); + ServiceResponse::ERROR_NOT_AVAILABLE, + "The target mode is not available. Please check the diagnostics."); } const auto req = std::make_shared(); req->mode = mode; From 7919138a777f0411fd613f2c797c612539a3534e Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 25 Jun 2024 10:40:47 +0900 Subject: [PATCH 55/64] get only the necessary one of object or pointcloud data --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 75e2b3d25086..2ceb9ea5410b 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -548,8 +548,14 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { const auto ego_odom_ptr = ego_odom_sub_.takeData(); - const auto objects_ptr = objects_sub_.takeData(); - const auto pointcloud_ptr = pointcloud_sub_.takeData(); + const auto [pointcloud_ptr, objects_ptr] = + [&]() -> std::pair { + if (use_pointcloud_) { + return std::make_pair(pointcloud_sub_.takeData(), nullptr); + } else { + return std::make_pair(nullptr, objects_sub_.takeData()); + } + }(); const auto acc_ptr = acc_sub_.takeData(); if ( !ego_odom_ptr || (!use_pointcloud_ && !objects_ptr) || (use_pointcloud_ && !pointcloud_ptr) || From be7d92f74c5857c4bc38f485a58618cab5f26ca6 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 25 Jun 2024 15:12:23 +0900 Subject: [PATCH 56/64] addfield for obstacle source type --- .../autoware/obstacle_cruise_planner/common_structs.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index fc8a0a96ade4..b9a8f5163a41 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -60,6 +60,7 @@ struct Obstacle : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + source_type(SourceType::PREDICTED_OBJECT), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), @@ -83,15 +84,22 @@ struct Obstacle : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + source_type(SourceType::POINTCLOUD), stop_collision_point(stop_collision_point), slow_down_front_collision_point(slow_down_front_collision_point), slow_down_back_collision_point(slow_down_back_collision_point) { } + enum class SourceType { + PREDICTED_OBJECT, + POINTCLOUD, + }; + rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; + SourceType source_type; // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp From 45d9da8331f5019f8e54d1b1a1667b56e5bc966f Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 25 Jun 2024 15:13:38 +0900 Subject: [PATCH 57/64] enable simultaneous use of PredictedObjects and PointCloud --- .../autoware/obstacle_cruise_planner/node.hpp | 12 +- .../src/node.cpp | 454 ++++++++---------- 2 files changed, 199 insertions(+), 267 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index d1570835b6c0..fce1165aa115 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -59,19 +59,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles( - const Odometry & odometry, const PredictedObjects & objects, - const std::vector & traj_points) const; - std::vector convertToObstacles( - const Odometry & odometry, const PointCloud2 & pointcloud, + const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, + const PointCloud2::ConstSharedPtr pointcloud_ptr, const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PredictedObjects & objects, - const std::vector & traj_points, const std::vector & obstacles); - std::tuple, std::vector, std::vector> - determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PointCloud2 & /* pointcloud */, + const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 2ceb9ea5410b..d58661e6cf6f 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -548,17 +548,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { const auto ego_odom_ptr = ego_odom_sub_.takeData(); - const auto [pointcloud_ptr, objects_ptr] = - [&]() -> std::pair { + const auto objects_ptr = objects_sub_.takeData(); + const auto pointcloud_ptr = [&]() -> PointCloud2::ConstSharedPtr { if (use_pointcloud_) { - return std::make_pair(pointcloud_sub_.takeData(), nullptr); + return pointcloud_sub_.takeData(); } else { - return std::make_pair(nullptr, objects_sub_.takeData()); + return nullptr; } }(); const auto acc_ptr = acc_sub_.takeData(); if ( - !ego_odom_ptr || (!use_pointcloud_ && !objects_ptr) || (use_pointcloud_ && !pointcloud_ptr) || + !ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || (use_pointcloud_ && !pointcloud_ptr))) || !acc_ptr) { return; } @@ -582,24 +582,12 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = [&]() { - if (use_pointcloud_) { - return convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); - } else { - return convertToObstacles(ego_odom, *objects_ptr, traj_points); - } - }(); + const auto target_obstacles = + convertToObstacles(ego_odom, objects_ptr, pointcloud_ptr, traj_points, msg->header); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. - const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { - if (use_pointcloud_) { - return determineEgoBehaviorAgainstObstacles( - ego_odom, *pointcloud_ptr, traj_points, target_obstacles); - } else { - return determineEgoBehaviorAgainstObstacles( - ego_odom, *objects_ptr, traj_points, target_obstacles); - } - }(); + const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = + determineEgoBehaviorAgainstObstacles(ego_odom, objects_ptr, traj_points, target_obstacles); // 3. Create data for planning const auto planner_data = createPlannerData(ego_odom, acc, traj_points); @@ -719,183 +707,174 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( } std::vector ObstacleCruisePlannerNode::convertToObstacles( - const Odometry & odometry, const PredictedObjects & objects, - const std::vector & traj_points) const + const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, + const PointCloud2::ConstSharedPtr pointcloud_ptr, + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects.objects) { - const auto & object_id = - autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); - const auto & current_obstacle_pose = - obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); - - // 1. Check if the obstacle's label is target - const uint8_t label = predicted_object.classification.front().label; - const bool is_target_obstacle = - isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); - if (!is_target_obstacle) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", - object_id.c_str()); - continue; - } - - // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - const auto ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); - if (!ego_to_obstacle_distance) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", - object_id.c_str()); - continue; - } - - // 3. Check if rough lateral distance is smaller than the threshold - const double lat_dist_from_obstacle_to_traj = - autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); - - const double min_lat_dist_to_traj_poly = [&]() { - const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); - return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - - obstacle_max_length; - }(); - - const double max_lat_margin = std::max( - {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, - p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); - if (max_lat_margin < min_lat_dist_to_traj_poly) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); - continue; - } - - const auto target_obstacle = Obstacle( - obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), - lat_dist_from_obstacle_to_traj); - target_obstacles.push_back(target_obstacle); - } - - const double calculation_time = stop_watch_.toc(__func__); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, - calculation_time); + if (objects_ptr) { + const auto obj_stamp = rclcpp::Time(objects_ptr->header.stamp); + + for (const auto & predicted_object : objects_ptr->objects) { + const auto & object_id = + autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + const auto & current_obstacle_pose = + obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); + + // 1. Check if the obstacle's label is target + const uint8_t label = predicted_object.classification.front().label; + const bool is_target_obstacle = + isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); + if (!is_target_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", + object_id.c_str()); + continue; + } - return target_obstacles; -} + // 2. Check if the obstacle is in front of the ego. + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + const auto ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", + object_id.c_str()); + continue; + } -std::vector ObstacleCruisePlannerNode::convertToObstacles( - const Odometry & odometry, const PointCloud2 & pointcloud, - const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const -{ - stop_watch_.tic(__func__); + // 3. Check if rough lateral distance is smaller than the threshold + const double lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); - const auto & p = behavior_determination_param_; + const double min_lat_dist_to_traj_poly = [&]() { + const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); + return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - + obstacle_max_length; + }(); - std::vector target_obstacles; + const double max_lat_margin = std::max( + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); + if (max_lat_margin < min_lat_dist_to_traj_poly) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } - // 1. transform pointcloud - std::optional transform_stamped{}; - try { - transform_stamped = tf_buffer_->lookupTransform( - traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " - << pointcloud.header.frame_id); - transform_stamped = std::nullopt; + const auto target_obstacle = Obstacle( + obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), + lat_dist_from_obstacle_to_traj); + target_obstacles.push_back(target_obstacle); + } } - if (transform_stamped && !pointcloud.data.empty()) { - PointCloud::Ptr pointcloud_ptr(new PointCloud); - pcl::fromROSMsg(pointcloud, *pointcloud_ptr); - const Eigen::Matrix4f transform = - tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); - pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); - - // 2. downsample & cluster pointcloud - PointCloud::Ptr filtered_points_ptr(new PointCloud); - pcl::VoxelGrid filter; - filter.setInputCloud(pointcloud_ptr); - filter.setLeafSize( - p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); - filter.filter(*filtered_points_ptr); - - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(filtered_points_ptr); - std::vector clusters; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(p.pointcloud_cluster_tolerance); - ec.setMinClusterSize(p.pointcloud_min_cluster_size); - ec.setMaxClusterSize(p.pointcloud_max_cluster_size); - ec.setSearchMethod(tree); - ec.setInputCloud(filtered_points_ptr); - ec.extract(clusters); - - const auto max_lat_margin = - std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - - // 3. convert clusters to obstacles - for (const auto & cluster_indices : clusters) { - double ego_to_stop_collision_distance = std::numeric_limits::max(); - double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); - double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); - double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); - double ego_to_obstacle_distance = std::numeric_limits::max(); - std::optional stop_collision_point = std::nullopt; - std::optional slow_down_front_collision_point = std::nullopt; - std::optional slow_down_back_collision_point = std::nullopt; - - for (const auto & index : cluster_indices.indices) { - const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); - const auto current_lat_dist_from_obstacle_to_traj = - autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); - const auto min_lat_dist_to_traj_poly = - std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; - - if (min_lat_dist_to_traj_poly < max_lat_margin) { - const auto current_ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); - if (current_ego_to_obstacle_distance) { - ego_to_obstacle_distance = - std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); - } else { - continue; - } - - lat_dist_from_obstacle_to_traj = - std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { + const auto & pointcloud = *pointcloud_ptr; + + // 1. transform pointcloud + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; + } - if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { - if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { - stop_collision_point = obstacle_point; - ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + if (transform_stamped) { + PointCloud::Ptr pcl_pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(pointcloud, *pcl_pointcloud_ptr); + const Eigen::Matrix4f transform = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl::transformPointCloud(*pcl_pointcloud_ptr, *pcl_pointcloud_ptr, transform); + + // 2. downsample & cluster pointcloud + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_pointcloud_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + + // 3. convert clusters to obstacles + for (const auto & cluster_indices : clusters) { + double ego_to_stop_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); + double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); + double ego_to_obstacle_distance = std::numeric_limits::max(); + std::optional stop_collision_point = std::nullopt; + std::optional slow_down_front_collision_point = std::nullopt; + std::optional slow_down_back_collision_point = std::nullopt; + + for (const auto & index : cluster_indices.indices) { + const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); + const auto current_lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < max_lat_margin) { + const auto current_ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); + } else { + continue; } - } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { - if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { - slow_down_front_collision_point = obstacle_point; - ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; - } else if ( - *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { - slow_down_back_collision_point = obstacle_point; - ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; + + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { + stop_collision_point = obstacle_point; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + } + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { + slow_down_front_collision_point = obstacle_point; + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { + slow_down_back_collision_point = obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; + } } } } - } - if (stop_collision_point || slow_down_front_collision_point) { - target_obstacles.emplace_back( - pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, - slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + if (stop_collision_point || slow_down_front_collision_point) { + target_obstacles.emplace_back( + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj); + } } } } @@ -954,7 +933,7 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( std::tuple, std::vector, std::vector> ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PredictedObjects & objects, + const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, const std::vector & traj_points, const std::vector & obstacles) { stop_watch_.tic(__func__); @@ -971,14 +950,21 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); - - // Calculate distance between trajectory and obstacle first - double precise_lat_dist = std::numeric_limits::max(); - for (const auto & traj_poly : decimated_traj_polys) { - const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); - precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); - } + const double precise_lat_dist = [&]() { + if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { + const auto obstacle_poly = + autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + // Calculate distance between trajectory and obstacle first + double precise_lat_dist = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); + precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); + } + return precise_lat_dist; + } else { + return obstacle.lat_dist_from_obstacle_to_traj; + } + }(); // Filter obstacles for cruise, stop and slow down const auto cruise_obstacle = @@ -1019,58 +1005,10 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } slow_down_condition_counter_.removeCounterUnlessUpdated(); - // Check target obstacles' consistency - checkConsistency(objects.header.stamp, objects, stop_obstacles); - - // update previous obstacles - prev_stop_obstacles_ = stop_obstacles; - prev_cruise_obstacles_ = cruise_obstacles; - prev_slow_down_obstacles_ = slow_down_obstacles; - - const double calculation_time = stop_watch_.toc(__func__); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", - __func__, calculation_time); - - return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; -} - -std::tuple, std::vector, std::vector> -ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PointCloud2 & /* pointcloud */, - const std::vector & traj_points, const std::vector & obstacles) -{ - stop_watch_.tic(__func__); - - // calculated decimated trajectory points and trajectory polygon - const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); - const auto decimated_traj_polys = - createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); - debug_data_ptr_->detection_polygons = decimated_traj_polys; - - // determine ego's behavior from stop, cruise and slow down - std::vector stop_obstacles; - std::vector cruise_obstacles; - std::vector slow_down_obstacles; - slow_down_condition_counter_.resetCurrentUuids(); - for (const auto & obstacle : obstacles) { - const double precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; - - // Filter obstacles for stop and slow down - const auto stop_obstacle = createStopObstacle( - odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); - if (stop_obstacle) { - stop_obstacles.push_back(*stop_obstacle); - continue; - } - const auto slow_down_obstacle = - createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); - if (slow_down_obstacle) { - slow_down_obstacles.push_back(*slow_down_obstacle); - continue; - } + if (objects_ptr) { + // Check target obstacles' consistency + checkConsistency(objects_ptr->header.stamp, *objects_ptr, stop_obstacles); } - slow_down_condition_counter_.removeCounterUnlessUpdated(); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -1114,7 +1052,7 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) { - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { return std::nullopt; } @@ -1162,7 +1100,7 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points) { - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { return std::nullopt; } @@ -1436,8 +1374,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // NOTE: consider all target obstacles when driving backward if ( - (!use_pointcloud_ && !isStopObstacle(obstacle.classification.label)) || - (use_pointcloud_ && !use_pointcloud_for_stop_)) { + (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT && + !isStopObstacle(obstacle.classification.label)) || + (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_stop_)) { return std::nullopt; } @@ -1451,14 +1390,14 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } std::optional> collision_point = std::nullopt; - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { if (obstacle.stop_collision_point) { const auto dist_to_collide_on_traj = autoware::motion_utils::calcSignedArcLength( traj_points, 0, obstacle.stop_collision_point.value()); collision_point = std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); } - } else { + } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { const auto & object_id = obstacle.uuid.substr(0, 4); // check obstacle velocity @@ -1526,10 +1465,11 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = [&]() { - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { return std::make_pair(0., 0.); + } else { + return projectObstacleVelocityToTrajectory(traj_points, obstacle); } - return projectObstacleVelocityToTrajectory(traj_points, obstacle); }(); return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape, tangent_vel, @@ -1543,20 +1483,21 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; - if (!use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); } if ( !enable_slow_down_planning_ || - (!use_pointcloud_ && !isSlowDownObstacle(obstacle.classification.label)) || - (use_pointcloud_ && !use_pointcloud_for_slow_down_)) { + (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT && + !isSlowDownObstacle(obstacle.classification.label)) || + (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_slow_down_)) { return std::nullopt; } std::optional front_collision_point = std::nullopt; std::optional back_collision_point = std::nullopt; - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { if (obstacle.slow_down_front_collision_point) { front_collision_point = obstacle.slow_down_front_collision_point; if (obstacle.slow_down_back_collision_point) { @@ -1565,7 +1506,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl back_collision_point = front_collision_point; } } - } else { + } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { const bool is_prev_obstacle_slow_down = getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); @@ -1676,10 +1617,11 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = [&]() { - if (use_pointcloud_) { + if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { return std::make_pair(0., 0.); + } else { + return projectObstacleVelocityToTrajectory(traj_points, obstacle); } - return projectObstacleVelocityToTrajectory(traj_points, obstacle); }(); return SlowDownObstacle{ obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, @@ -1690,10 +1632,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - if (use_pointcloud_) { - return; - } - for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), From 5d5ec3f6d0b213d2ee5501b202870a35c2807beb Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 10:08:15 +0900 Subject: [PATCH 58/64] separate convertToObstacles() by source type --- .../autoware/obstacle_cruise_planner/node.hpp | 6 +- .../src/node.cpp | 328 +++++++++--------- 2 files changed, 176 insertions(+), 158 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index fce1165aa115..21962b835df1 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -59,8 +59,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles( - const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, - const PointCloud2::ConstSharedPtr pointcloud_ptr, + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index d58661e6cf6f..ca49ce1dc9d4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -549,13 +549,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms { const auto ego_odom_ptr = ego_odom_sub_.takeData(); const auto objects_ptr = objects_sub_.takeData(); - const auto pointcloud_ptr = [&]() -> PointCloud2::ConstSharedPtr { - if (use_pointcloud_) { - return pointcloud_sub_.takeData(); - } else { - return nullptr; - } - }(); + const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; const auto acc_ptr = acc_sub_.takeData(); if ( !ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || (use_pointcloud_ && !pointcloud_ptr))) || @@ -582,8 +576,21 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = - convertToObstacles(ego_odom, objects_ptr, pointcloud_ptr, traj_points, msg->header); + const auto target_obstacles = [&]() { + std::vector target_obstacles; + if (objects_ptr) { + const auto object_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); + target_obstacles.insert( + target_obstacles.end(), object_obstacles.begin(), object_obstacles.end()); + } + if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { + const auto pointcloud_obstacles = + convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); + target_obstacles.insert( + target_obstacles.end(), pointcloud_obstacles.begin(), pointcloud_obstacles.end()); + } + return target_obstacles; + }(); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = @@ -707,174 +714,183 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( } std::vector ObstacleCruisePlannerNode::convertToObstacles( - const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, - const PointCloud2::ConstSharedPtr pointcloud_ptr, - const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points) const { stop_watch_.tic(__func__); + const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - if (objects_ptr) { - const auto obj_stamp = rclcpp::Time(objects_ptr->header.stamp); - - for (const auto & predicted_object : objects_ptr->objects) { - const auto & object_id = - autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); - const auto & current_obstacle_pose = - obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); - - // 1. Check if the obstacle's label is target - const uint8_t label = predicted_object.classification.front().label; - const bool is_target_obstacle = - isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); - if (!is_target_obstacle) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", - object_id.c_str()); - continue; - } - - // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - const auto ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); - if (!ego_to_obstacle_distance) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", - object_id.c_str()); - continue; - } + for (const auto & predicted_object : objects.objects) { + const auto & object_id = + autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + const auto & current_obstacle_pose = + obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); + + // 1. Check if the obstacle's label is target + const uint8_t label = predicted_object.classification.front().label; + const bool is_target_obstacle = + isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); + if (!is_target_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", + object_id.c_str()); + continue; + } - // 3. Check if rough lateral distance is smaller than the threshold - const double lat_dist_from_obstacle_to_traj = - autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + // 2. Check if the obstacle is in front of the ego. + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + const auto ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", + object_id.c_str()); + continue; + } - const double min_lat_dist_to_traj_poly = [&]() { - const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); - return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - - obstacle_max_length; - }(); + // 3. Check if rough lateral distance is smaller than the threshold + const double lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); - const double max_lat_margin = std::max( - {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, - p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); - if (max_lat_margin < min_lat_dist_to_traj_poly) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); - continue; - } + const double min_lat_dist_to_traj_poly = [&]() { + const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); + return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - + obstacle_max_length; + }(); - const auto target_obstacle = Obstacle( - obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), - lat_dist_from_obstacle_to_traj); - target_obstacles.push_back(target_obstacle); + const double max_lat_margin = std::max( + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); + if (max_lat_margin < min_lat_dist_to_traj_poly) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; } + + const auto target_obstacle = Obstacle( + obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), + lat_dist_from_obstacle_to_traj); + target_obstacles.push_back(target_obstacle); } - if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { - const auto & pointcloud = *pointcloud_ptr; + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, + calculation_time); + + return target_obstacles; +} +std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const +{ + stop_watch_.tic(__func__); + + const auto & p = behavior_determination_param_; + + std::vector target_obstacles; + + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; + } + + if (transform_stamped) { // 1. transform pointcloud - std::optional transform_stamped{}; - try { - transform_stamped = tf_buffer_->lookupTransform( - traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " - << pointcloud.header.frame_id); - transform_stamped = std::nullopt; - } + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(pointcloud, *pointcloud_ptr); + const Eigen::Matrix4f transform = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); + + // 2. downsample & cluster pointcloud + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + + // 3. convert clusters to obstacles + for (const auto & cluster_indices : clusters) { + double ego_to_stop_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); + double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); + double ego_to_obstacle_distance = std::numeric_limits::max(); + std::optional stop_collision_point = std::nullopt; + std::optional slow_down_front_collision_point = std::nullopt; + std::optional slow_down_back_collision_point = std::nullopt; + + for (const auto & index : cluster_indices.indices) { + const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); + const auto current_lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < max_lat_margin) { + const auto current_ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); + } else { + continue; + } - if (transform_stamped) { - PointCloud::Ptr pcl_pointcloud_ptr(new PointCloud); - pcl::fromROSMsg(pointcloud, *pcl_pointcloud_ptr); - const Eigen::Matrix4f transform = - tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); - pcl::transformPointCloud(*pcl_pointcloud_ptr, *pcl_pointcloud_ptr, transform); - - // 2. downsample & cluster pointcloud - PointCloud::Ptr filtered_points_ptr(new PointCloud); - pcl::VoxelGrid filter; - filter.setInputCloud(pcl_pointcloud_ptr); - filter.setLeafSize( - p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); - filter.filter(*filtered_points_ptr); - - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(filtered_points_ptr); - std::vector clusters; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(p.pointcloud_cluster_tolerance); - ec.setMinClusterSize(p.pointcloud_min_cluster_size); - ec.setMaxClusterSize(p.pointcloud_max_cluster_size); - ec.setSearchMethod(tree); - ec.setInputCloud(filtered_points_ptr); - ec.extract(clusters); - - const auto max_lat_margin = - std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); - - // 3. convert clusters to obstacles - for (const auto & cluster_indices : clusters) { - double ego_to_stop_collision_distance = std::numeric_limits::max(); - double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); - double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); - double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); - double ego_to_obstacle_distance = std::numeric_limits::max(); - std::optional stop_collision_point = std::nullopt; - std::optional slow_down_front_collision_point = std::nullopt; - std::optional slow_down_back_collision_point = std::nullopt; - - for (const auto & index : cluster_indices.indices) { - const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); - const auto current_lat_dist_from_obstacle_to_traj = - autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); - const auto min_lat_dist_to_traj_poly = - std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; - - if (min_lat_dist_to_traj_poly < max_lat_margin) { - const auto current_ego_to_obstacle_distance = - calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); - if (current_ego_to_obstacle_distance) { - ego_to_obstacle_distance = - std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); - } else { - continue; - } + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); - lat_dist_from_obstacle_to_traj = - std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); - - if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { - if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { - stop_collision_point = obstacle_point; - ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; - } - } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { - if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { - slow_down_front_collision_point = obstacle_point; - ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; - } else if ( - *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { - slow_down_back_collision_point = obstacle_point; - ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; - } + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { + stop_collision_point = obstacle_point; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + } + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { + slow_down_front_collision_point = obstacle_point; + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { + slow_down_back_collision_point = obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; } } } + } - if (stop_collision_point || slow_down_front_collision_point) { - target_obstacles.emplace_back( - pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, - slow_down_back_collision_point, ego_to_obstacle_distance, - lat_dist_from_obstacle_to_traj); - } + if (stop_collision_point || slow_down_front_collision_point) { + target_obstacles.emplace_back( + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); } } } From 3b336d7673128e1dabfdea487ff698dff211aef9 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 10:37:48 +0900 Subject: [PATCH 59/64] avoid using pointer --- .../autoware/obstacle_cruise_planner/node.hpp | 2 +- .../autoware_obstacle_cruise_planner/src/node.cpp | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 21962b835df1..908b72744c8e 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -67,7 +67,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, + const Odometry & odometry, const std::optional & objects, const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index ca49ce1dc9d4..22b58f4584d4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -559,6 +559,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto & ego_odom = *ego_odom_ptr; const auto & acc = *acc_ptr; + const auto & objects = objects_ptr ? std::make_optional(*objects_ptr) : std::nullopt; const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready @@ -578,8 +579,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (3) not too far from trajectory const auto target_obstacles = [&]() { std::vector target_obstacles; - if (objects_ptr) { - const auto object_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); + if (objects) { + const auto object_obstacles = convertToObstacles(ego_odom, *objects, traj_points); target_obstacles.insert( target_obstacles.end(), object_obstacles.begin(), object_obstacles.end()); } @@ -594,7 +595,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(ego_odom, objects_ptr, traj_points, target_obstacles); + determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); // 3. Create data for planning const auto planner_data = createPlannerData(ego_odom, acc, traj_points); @@ -949,7 +950,7 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( std::tuple, std::vector, std::vector> ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PredictedObjects::ConstSharedPtr objects_ptr, + const Odometry & odometry, const std::optional & objects, const std::vector & traj_points, const std::vector & obstacles) { stop_watch_.tic(__func__); @@ -1021,9 +1022,9 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } slow_down_condition_counter_.removeCounterUnlessUpdated(); - if (objects_ptr) { + if (objects) { // Check target obstacles' consistency - checkConsistency(objects_ptr->header.stamp, *objects_ptr, stop_obstacles); + checkConsistency(objects->header.stamp, *objects, stop_obstacles); } // update previous obstacles From eec09a70f8f49b2503715b49e11396d8e26352af Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 11:01:58 +0900 Subject: [PATCH 60/64] reduce diff --- .../autoware/obstacle_cruise_planner/node.hpp | 2 +- .../autoware_obstacle_cruise_planner/src/node.cpp | 15 ++++++--------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 908b72744c8e..826f14ba410f 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -123,7 +123,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; - bool enable_slow_down_planning_{false}; bool use_pointcloud_for_stop_; bool use_pointcloud_for_slow_down_; double min_behavior_stop_margin_; @@ -292,6 +291,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; + bool enable_slow_down_planning_{false}; bool use_pointcloud_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 22b58f4584d4..3ee92f9bfd96 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -558,8 +558,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms } const auto & ego_odom = *ego_odom_ptr; - const auto & acc = *acc_ptr; const auto & objects = objects_ptr ? std::make_optional(*objects_ptr) : std::nullopt; + const auto & acc = *acc_ptr; const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready @@ -1073,9 +1073,8 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( return std::nullopt; } - const auto & p = behavior_determination_param_; - const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; // NOTE: When driving backward, Stop will be planned instead of cruise. // When the obstacle is crossing the ego's trajectory, cruise can be ignored. @@ -1121,11 +1120,10 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac return std::nullopt; } - const auto & p = behavior_determination_param_; - if (traj_points.empty()) return std::nullopt; // check label const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; if (!isOutsideCruiseObstacle(obstacle.classification.label)) { RCLCPP_INFO_EXPRESSION( @@ -1409,10 +1407,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::optional> collision_point = std::nullopt; if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { if (obstacle.stop_collision_point) { - const auto dist_to_collide_on_traj = autoware::motion_utils::calcSignedArcLength( - traj_points, 0, obstacle.stop_collision_point.value()); - collision_point = - std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); + const auto dist_to_collide_on_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); + collision_point = std::make_pair(*obstacle.stop_collision_point, dist_to_collide_on_traj); } } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { const auto & object_id = obstacle.uuid.substr(0, 4); From 61ee56a35b44dc64870780b566076c0f25fe4e1f Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 13:53:45 +0900 Subject: [PATCH 61/64] make nest shallower --- .../common_structs.hpp | 8 - .../autoware/obstacle_cruise_planner/node.hpp | 17 +- .../src/node.cpp | 544 ++++++++++-------- 3 files changed, 311 insertions(+), 258 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index b9a8f5163a41..fc8a0a96ade4 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -60,7 +60,6 @@ struct Obstacle : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), - source_type(SourceType::PREDICTED_OBJECT), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), @@ -84,22 +83,15 @@ struct Obstacle : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), - source_type(SourceType::POINTCLOUD), stop_collision_point(stop_collision_point), slow_down_front_collision_point(slow_down_front_collision_point), slow_down_back_collision_point(slow_down_back_collision_point) { } - enum class SourceType { - PREDICTED_OBJECT, - POINTCLOUD, - }; - rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; - SourceType source_type; // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 826f14ba410f..b96605f57b94 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -66,15 +66,22 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> - determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const std::optional & objects, + determineEgoBehaviorAgainstPredictedObjectObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; - std::optional createStopObstacle( + std::optional createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lateral_dist) const; + std::optional createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lateral_dist) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; @@ -101,9 +108,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double calcCollisionTimeMargin( const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const; - std::optional createSlowDownObstacle( + std::optional createSlowDownObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const Obstacle & obstacle, const double precise_lat_dist); + std::optional createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist); PlannerData createPlannerData( const Odometry & odometry, const AccelWithCovarianceStamped & acc, const std::vector & traj_points) const; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 3ee92f9bfd96..0eb5ea545579 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -558,7 +558,6 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms } const auto & ego_odom = *ego_odom_ptr; - const auto & objects = objects_ptr ? std::make_optional(*objects_ptr) : std::nullopt; const auto & acc = *acc_ptr; const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); @@ -573,30 +572,49 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto is_driving_forward = autoware::motion_utils::isDrivingForwardWithTwist(traj_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; - // 1. Convert predicted objects to obstacles which are - // (1) with a proper label - // (2) in front of ego - // (3) not too far from trajectory - const auto target_obstacles = [&]() { - std::vector target_obstacles; - if (objects) { - const auto object_obstacles = convertToObstacles(ego_odom, *objects, traj_points); - target_obstacles.insert( - target_obstacles.end(), object_obstacles.begin(), object_obstacles.end()); + const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { + std::vector stop_obstacles; + std::vector cruise_obstacles; + std::vector slow_down_obstacles; + if (objects_ptr) { + // 1. Convert predicted objects to obstacles which are + // (1) with a proper label + // (2) in front of ego + // (3) not too far from trajectory + const auto target_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); + + // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. + const auto & [stop_object_obstacles, cruise_object_obstacles, slow_down_object_obstacles] = + determineEgoBehaviorAgainstPredictedObjectObstacles( + ego_odom, *objects_ptr, traj_points, target_obstacles); + + stop_obstacles.insert( + stop_obstacles.end(), stop_object_obstacles.begin(), stop_object_obstacles.end()); + cruise_obstacles.insert( + cruise_obstacles.end(), cruise_object_obstacles.begin(), cruise_object_obstacles.end()); + slow_down_obstacles.insert( + slow_down_obstacles.end(), slow_down_object_obstacles.begin(), + slow_down_object_obstacles.end()); } if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { - const auto pointcloud_obstacles = + const auto target_obstacles = convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); - target_obstacles.insert( - target_obstacles.end(), pointcloud_obstacles.begin(), pointcloud_obstacles.end()); + + const auto & [stop_pointcloud_obstacles, cruise_pointcloud_obstacles, slow_down_pointcloud_obstacles] = + determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles); + + stop_obstacles.insert( + stop_obstacles.end(), stop_pointcloud_obstacles.begin(), stop_pointcloud_obstacles.end()); + cruise_obstacles.insert( + cruise_obstacles.end(), cruise_pointcloud_obstacles.begin(), + cruise_pointcloud_obstacles.end()); + slow_down_obstacles.insert( + slow_down_obstacles.end(), slow_down_pointcloud_obstacles.begin(), + slow_down_pointcloud_obstacles.end()); } - return target_obstacles; + return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles); }(); - // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. - const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); - // 3. Create data for planning const auto planner_data = createPlannerData(ego_odom, acc, traj_points); @@ -949,8 +967,8 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( } std::tuple, std::vector, std::vector> -ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const std::optional & objects, +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles) { stop_watch_.tic(__func__); @@ -967,21 +985,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const double precise_lat_dist = [&]() { - if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { - const auto obstacle_poly = - autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); - // Calculate distance between trajectory and obstacle first - double precise_lat_dist = std::numeric_limits::max(); - for (const auto & traj_poly : decimated_traj_polys) { - const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); - precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); - } - return precise_lat_dist; - } else { - return obstacle.lat_dist_from_obstacle_to_traj; - } - }(); + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + + // Calculate distance between trajectory and obstacle first + double precise_lat_dist = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); + precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); + } // Filter obstacles for cruise, stop and slow down const auto cruise_obstacle = @@ -990,14 +1001,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( cruise_obstacles.push_back(*cruise_obstacle); continue; } - const auto stop_obstacle = createStopObstacle( + const auto stop_obstacle = createStopObstacleForPredictedObject( odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; } - const auto slow_down_obstacle = - createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); + const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject( + odometry, decimated_traj_points, obstacle, precise_lat_dist); if (slow_down_obstacle) { slow_down_obstacles.push_back(*slow_down_obstacle); continue; @@ -1022,10 +1033,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } slow_down_condition_counter_.removeCounterUnlessUpdated(); - if (objects) { - // Check target obstacles' consistency - checkConsistency(objects->header.stamp, *objects, stop_obstacles); - } + // Check target obstacles' consistency + checkConsistency(objects.header.stamp, objects, stop_obstacles); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -1040,6 +1049,47 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; } +std::tuple, std::vector, std::vector> +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles) +{ + stop_watch_.tic(__func__); + + // calculated decimated trajectory points and trajectory polygon + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); + const auto decimated_traj_polys = + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); + debug_data_ptr_->detection_polygons = decimated_traj_polys; + + // determine ego's behavior from stop and slow down + std::vector stop_obstacles; + std::vector slow_down_obstacles; + for (const auto & obstacle : obstacles) { + const auto & precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; + + // Filter obstacles for stop and slow down + const auto stop_obstacle = + createStopObstacleForPointCloud(decimated_traj_points, obstacle, precise_lat_dist); + if (stop_obstacle) { + stop_obstacles.push_back(*stop_obstacle); + continue; + } + const auto slow_down_obstacle = createSlowDownObstacleForPointCloud(obstacle, precise_lat_dist); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); + + return {stop_obstacles, {}, slow_down_obstacles}; +} + std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const { @@ -1069,10 +1119,6 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) { - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - return std::nullopt; - } - const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; @@ -1116,10 +1162,6 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points) { - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - return std::nullopt; - } - if (traj_points.empty()) return std::nullopt; // check label const auto & object_id = obstacle.uuid.substr(0, 4); @@ -1379,7 +1421,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return collision_points; } -std::optional ObstacleCruisePlannerNode::createStopObstacle( +std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) const @@ -1388,10 +1430,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( const auto & object_id = obstacle.uuid.substr(0, 4); // NOTE: consider all target obstacles when driving backward - if ( - (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT && - !isStopObstacle(obstacle.classification.label)) || - (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_stop_)) { + if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } @@ -1404,242 +1443,255 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } - std::optional> collision_point = std::nullopt; - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - if (obstacle.stop_collision_point) { - const auto dist_to_collide_on_traj = - autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); - collision_point = std::make_pair(*obstacle.stop_collision_point, dist_to_collide_on_traj); + // check obstacle velocity + // NOTE: If precise_lat_dist is 0, always plan stop + constexpr double epsilon = 1e-6; + if (epsilon < precise_lat_dist) { + const auto [obstacle_tangent_vel, obstacle_normal_vel] = + projectObstacleVelocityToTrajectory(traj_points, obstacle); + if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { + return std::nullopt; } - } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { - const auto & object_id = obstacle.uuid.substr(0, 4); + } - // check obstacle velocity - // NOTE: If precise_lat_dist is 0, always plan stop - constexpr double epsilon = 1e-6; - if (epsilon < precise_lat_dist) { - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { - return std::nullopt; - } + // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, + // and the collision between ego and obstacles are within the margin threshold. + const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); + const bool has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + if (is_obstacle_crossing && has_high_speed) { + // Get highest confidence predicted path + const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon); + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; } - // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, - // and the collision between ego and obstacles are within the margin threshold. - const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const bool has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - if (is_obstacle_crossing && has_high_speed) { - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( - obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); - - std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); - if (collision_points.empty()) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " - "predicted path " - "and the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; - } - - const double collision_time_margin = - calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); - if (p.collision_time_margin < collision_time_margin) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); - return std::nullopt; - } + const double collision_time_margin = + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; } + } - // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); + // calculate collision points with trajectory with lateral stop margin + const auto traj_polys_with_lat_margin = + createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); - collision_point = polygon_utils::getCollisionPoint( - traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); - } + const auto collision_point = polygon_utils::getCollisionPoint( + traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); if (!collision_point) { return std::nullopt; } - const auto [tangent_vel, normal_vel] = [&]() { - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - return std::make_pair(0., 0.); - } else { - return projectObstacleVelocityToTrajectory(traj_points, obstacle); - } - }(); + const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, collision_point->first, collision_point->second}; } -std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( +std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lat_dist) const +{ + const auto & p = behavior_determination_param_; + + if (!use_pointcloud_for_stop_) { + return std::nullopt; + } + + if (!obstacle.stop_collision_point) { + return std::nullopt; + } + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { + return std::nullopt; + } + + const auto dist_to_collide_on_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + {}, + {}, + *obstacle.stop_collision_point, + dist_to_collide_on_traj}; +} + +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const Obstacle & obstacle, const double precise_lat_dist) { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; + slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); - if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { - slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); - } + const bool is_prev_obstacle_slow_down = + getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); - if ( - !enable_slow_down_planning_ || - (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT && - !isSlowDownObstacle(obstacle.classification.label)) || - (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_slow_down_)) { + if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { return std::nullopt; } - std::optional front_collision_point = std::nullopt; - std::optional back_collision_point = std::nullopt; - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - if (obstacle.slow_down_front_collision_point) { - front_collision_point = obstacle.slow_down_front_collision_point; - if (obstacle.slow_down_back_collision_point) { - back_collision_point = obstacle.slow_down_back_collision_point; - } else { - back_collision_point = front_collision_point; + // check lateral distance considering hysteresis + const bool is_lat_dist_low = isLowerConsideringHysteresis( + precise_lat_dist, is_prev_obstacle_slow_down, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, + p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obstacle.uuid); + return false; + } } + return true; } - } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { - const bool is_prev_obstacle_slow_down = - getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); - - // check lateral distance considering hysteresis - const bool is_lat_dist_low = isLowerConsideringHysteresis( - precise_lat_dist, is_prev_obstacle_slow_down, - p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, - p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); - - const bool is_slow_down_required = [&]() { - if (is_prev_obstacle_slow_down) { - // check if exiting slow down - if (!is_lat_dist_low) { - const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); - if (count <= -p.successive_num_to_exit_slow_down_condition) { - slow_down_condition_counter_.reset(obstacle.uuid); - return false; - } - } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obstacle.uuid); return true; } - // check if entering slow down - if (is_lat_dist_low) { - const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); - if (p.successive_num_to_entry_slow_down_condition <= count) { - slow_down_condition_counter_.reset(obstacle.uuid); - return true; - } - } - return false; - }(); - if (!is_slow_down_required) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", - object_id.c_str(), precise_lat_dist); - return std::nullopt; } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), + precise_lat_dist); + return std::nullopt; + } - const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); - - // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not divided by two. - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, - p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); - - std::vector front_collision_polygons; - size_t front_seg_idx = 0; - std::vector back_collision_polygons; - size_t back_seg_idx = 0; - for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) { - std::vector collision_polygons; - bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); - - if (!collision_polygons.empty()) { - if (front_collision_polygons.empty()) { - front_collision_polygons = collision_polygons; - front_seg_idx = i == 0 ? i : i - 1; - } - back_collision_polygons = collision_polygons; - back_seg_idx = i == 0 ? i : i - 1; - } else { - if (!back_collision_polygons.empty()) { - break; // for efficient calculation - } + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + + // calculate collision points with trajectory with lateral stop margin + // NOTE: For additional margin, hysteresis is not divided by two. + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, odometry.pose.pose, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); + + std::vector front_collision_polygons; + size_t front_seg_idx = 0; + std::vector back_collision_polygons; + size_t back_seg_idx = 0; + for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) { + std::vector collision_polygons; + bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); + + if (!collision_polygons.empty()) { + if (front_collision_polygons.empty()) { + front_collision_polygons = collision_polygons; + front_seg_idx = i == 0 ? i : i - 1; + } + back_collision_polygons = collision_polygons; + back_seg_idx = i == 0 ? i : i - 1; + } else { + if (!back_collision_polygons.empty()) { + break; // for efficient calculation } } + } - if (front_collision_polygons.empty() || back_collision_polygons.empty()) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str()); - return std::nullopt; - } + if (front_collision_polygons.empty() || back_collision_polygons.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str()); + return std::nullopt; + } - // calculate front collision point - double front_min_dist = std::numeric_limits::max(); - for (const auto & collision_poly : front_collision_polygons) { - for (const auto & collision_point : collision_poly.outer()) { - const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( - traj_points, front_seg_idx, collision_geom_point); - if (dist < front_min_dist) { - front_min_dist = dist; - front_collision_point = collision_geom_point; - } + // calculate front collision point + double front_min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point front_collision_point; + for (const auto & collision_poly : front_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, front_seg_idx, collision_geom_point); + if (dist < front_min_dist) { + front_min_dist = dist; + front_collision_point = collision_geom_point; } } + } - // calculate back collision point - double back_max_dist = -std::numeric_limits::max(); - back_collision_point = front_collision_point; - for (const auto & collision_poly : back_collision_polygons) { - for (const auto & collision_point : collision_poly.outer()) { - const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( - traj_points, back_seg_idx, collision_geom_point); - if (back_max_dist < dist) { - back_max_dist = dist; - back_collision_point = collision_geom_point; - } + // calculate back collision point + double back_max_dist = -std::numeric_limits::max(); + geometry_msgs::msg::Point back_collision_point = front_collision_point; + for (const auto & collision_poly : back_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, back_seg_idx, collision_geom_point); + if (back_max_dist < dist) { + back_max_dist = dist; + back_collision_point = collision_geom_point; } } } - if (!front_collision_point || !back_collision_point) { + + const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, tangent_vel, normal_vel, + precise_lat_dist, front_collision_point, back_collision_point}; +} + +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist) +{ + if (!enable_slow_down_planning_ || !use_pointcloud_for_slow_down_) { + return std::nullopt; + } + + if (!obstacle.slow_down_front_collision_point) { return std::nullopt; } - const auto [tangent_vel, normal_vel] = [&]() { - if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { - return std::make_pair(0., 0.); - } else { - return projectObstacleVelocityToTrajectory(traj_points, obstacle); - } - }(); + auto front_collision_point = *obstacle.slow_down_front_collision_point; + auto back_collision_point = + obstacle.slow_down_back_collision_point.value_or(front_collision_point); + return SlowDownObstacle{ - obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, tangent_vel, - normal_vel, precise_lat_dist, *front_collision_point, *back_collision_point}; + obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, {}, {}, + precise_lat_dist, front_collision_point, back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( From 9b313ddcafe1d05aec16d97a10257bd2ea358989 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 14:52:16 +0900 Subject: [PATCH 62/64] define vector concatenate function --- .../src/node.cpp | 27 +++++++++---------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 0eb5ea545579..7c25cb5736bc 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -232,6 +232,12 @@ bool isLowerConsideringHysteresis( } return false; } + +template +void concatenate(std::vector & first, const std::vector & last) +{ + first.insert(first.end(), last.begin(), last.end()); +} } // namespace namespace autoware::motion_planning @@ -588,13 +594,9 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms determineEgoBehaviorAgainstPredictedObjectObstacles( ego_odom, *objects_ptr, traj_points, target_obstacles); - stop_obstacles.insert( - stop_obstacles.end(), stop_object_obstacles.begin(), stop_object_obstacles.end()); - cruise_obstacles.insert( - cruise_obstacles.end(), cruise_object_obstacles.begin(), cruise_object_obstacles.end()); - slow_down_obstacles.insert( - slow_down_obstacles.end(), slow_down_object_obstacles.begin(), - slow_down_object_obstacles.end()); + concatenate(stop_obstacles, stop_object_obstacles); + concatenate(cruise_obstacles, cruise_object_obstacles); + concatenate(slow_down_obstacles, slow_down_object_obstacles); } if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { const auto target_obstacles = @@ -603,14 +605,9 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto & [stop_pointcloud_obstacles, cruise_pointcloud_obstacles, slow_down_pointcloud_obstacles] = determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles); - stop_obstacles.insert( - stop_obstacles.end(), stop_pointcloud_obstacles.begin(), stop_pointcloud_obstacles.end()); - cruise_obstacles.insert( - cruise_obstacles.end(), cruise_pointcloud_obstacles.begin(), - cruise_pointcloud_obstacles.end()); - slow_down_obstacles.insert( - slow_down_obstacles.end(), slow_down_pointcloud_obstacles.begin(), - slow_down_pointcloud_obstacles.end()); + concatenate(stop_obstacles, stop_pointcloud_obstacles); + concatenate(cruise_obstacles, cruise_pointcloud_obstacles); + concatenate(slow_down_obstacles, slow_down_pointcloud_obstacles); } return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles); }(); From a4a5986bcabf23528db2ec486f183d5ea7f0a722 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 2 Jul 2024 14:46:21 +0900 Subject: [PATCH 63/64] shorten variable names --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 7c25cb5736bc..f9451104259b 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -602,12 +602,12 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto target_obstacles = convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); - const auto & [stop_pointcloud_obstacles, cruise_pointcloud_obstacles, slow_down_pointcloud_obstacles] = + const auto & [stop_pc_obstacles, cruise_pc_obstacles, slow_down_pc_obstacles] = determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles); - concatenate(stop_obstacles, stop_pointcloud_obstacles); - concatenate(cruise_obstacles, cruise_pointcloud_obstacles); - concatenate(slow_down_obstacles, slow_down_pointcloud_obstacles); + concatenate(stop_obstacles, stop_pc_obstacles); + concatenate(cruise_obstacles, cruise_pc_obstacles); + concatenate(slow_down_obstacles, slow_down_pc_obstacles); } return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles); }(); From 7f227b6b7cba739a7bc0c590f646a23fe06ea127 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 2 Jul 2024 14:56:03 +0900 Subject: [PATCH 64/64] fix redundant condition --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index f9451104259b..c8f962c3ad71 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -557,9 +557,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto objects_ptr = objects_sub_.takeData(); const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; const auto acc_ptr = acc_sub_.takeData(); - if ( - !ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || (use_pointcloud_ && !pointcloud_ptr))) || - !acc_ptr) { + if (!ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || !pointcloud_ptr)) || !acc_ptr) { return; }