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 ae43c2d81efb..d0c34ea01b2c 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
@@ -160,6 +160,7 @@
+
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 8b338e9d0cd9..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
@@ -37,6 +37,7 @@
motorcycle: true
bicycle: true
pedestrian: true
+ pointcloud: false
cruise_obstacle_type:
inside:
@@ -68,8 +69,17 @@
motorcycle: true
bicycle: true
pedestrian: true
+ pointcloud: false
behavior_determination:
+ 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
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 e4ae670ef6b9..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
@@ -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 dd9a6c388067..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
@@ -27,6 +27,9 @@
#include
#include
+#include
+#include
+
#include
#include
#include
@@ -58,16 +61,27 @@ 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(
+ 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;
@@ -94,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;
@@ -116,6 +132,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
bool enable_debug_info_;
bool enable_calculation_time_info_;
+ 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_;
@@ -149,9 +167,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 +205,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;
@@ -271,6 +301,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
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/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp
index 0641d076bb3f..3679c253e2bb 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/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp
index 5721efd051f6..03e536ce238c 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 f6e7c2bb43b3..c8f962c3ad71 100644
--- a/planning/autoware_obstacle_cruise_planner/src/node.cpp
+++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp
@@ -23,8 +23,15 @@
#include "autoware/universe_utils/ros/update_param.hpp"
#include "object_recognition_utils/predicted_path_utils.hpp"
+#include
+#include
+
#include
+#include
+#include
+#include
+
#include
#include
@@ -194,6 +201,15 @@ std::vector resampleTrajectoryPoints(
return autoware::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 autoware::universe_utils::Point2d & point)
{
geometry_msgs::msg::Point geom_point;
@@ -216,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
@@ -225,6 +247,20 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
{ // behavior determination
decimate_trajectory_step_length =
node.declare_parameter("behavior_determination.decimate_trajectory_step_length");
+ 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(
@@ -284,6 +320,21 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
autoware::universe_utils::updateParam(
parameters, "behavior_determination.decimate_trajectory_step_length",
decimate_trajectory_step_length);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_cluster_tolerance",
+ pointcloud_cluster_tolerance);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size);
+ autoware::universe_utils::updateParam(
+ parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size);
autoware::universe_utils::updateParam(
parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold",
crossing_obstacle_velocity_threshold);
@@ -387,6 +438,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);
@@ -395,6 +450,11 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info");
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);
{ // planning algorithm
@@ -422,7 +482,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_);
}
@@ -476,7 +536,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_);
@@ -495,13 +555,13 @@ 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 = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr;
const auto acc_ptr = acc_sub_.takeData();
- if (!ego_odom_ptr || !objects_ptr || !acc_ptr) {
+ if (!ego_odom_ptr || (!objects_ptr && (!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 = autoware::motion_utils::convertToTrajectoryPointArray(*msg);
@@ -516,15 +576,39 @@ 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 = convertToObstacles(ego_odom, objects, traj_points);
+ 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);
+
+ 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 =
+ convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header);
+
+ const auto & [stop_pc_obstacles, cruise_pc_obstacles, slow_down_pc_obstacles] =
+ determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, 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] =
- determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_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);
+ }();
// 3. Create data for planning
const auto planner_data = createPlannerData(ego_odom, acc, traj_points);
@@ -715,6 +799,124 @@ 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;
+
+ 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
+ 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 (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);
+ }
+ }
+ }
+
+ 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_;
@@ -760,7 +962,7 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle(
}
std::tuple, std::vector, std::vector>
-ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
+ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles(
const Odometry & odometry, const PredictedObjects & objects,
const std::vector & traj_points, const std::vector & obstacles)
{
@@ -778,7 +980,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 = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape);
// Calculate distance between trajectory and obstacle first
double precise_lat_dist = std::numeric_limits::max();
@@ -794,14 +996,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;
@@ -842,6 +1044,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
{
@@ -1173,7 +1416,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
@@ -1264,7 +1507,45 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle(
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)
{
@@ -1315,7 +1596,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl
return std::nullopt;
}
- const auto obstacle_poly = obstacle.toPolygon();
+ 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.
@@ -1388,6 +1669,26 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl
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;
+ }
+
+ 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, {}, {},
+ precise_lat_dist, front_collision_point, back_collision_point};
+}
+
void ObstacleCruisePlannerNode::checkConsistency(
const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
std::vector & stop_obstacles)
diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
index 516bf5579b82..0413d4bbf352 100644
--- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
+++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
@@ -439,7 +439,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;
}