Skip to content

Commit a0e04f7

Browse files
mitukou1109pre-commit-ci[bot]isamu-takagisatoshi-ota
authored and
KhalilSelyan
committed
feat(obstacle_cruise_planner): support pointcloud-based obstacles (#6907)
* add pointcloud to obstacle properties * add tf listener & pointcloud subscriber * add parameters for pointcloud obstacle * add type aliases * convert pointcloud to obstacle * add type alias * add polygon conversion for pointcloud obstacle * initialize twist & pose of pointcloud obstacle * overload to handle both obstacle & predicted path * implement ego behavior determination against pointcloud obstacles * generate obstacle from point * revert getCollisionIndex() * generate obstacle from each point in cloud * set pointcloud obstacle velocity to 0 * use tf buffer & listener with pointers * update latest pointcloud data * add topic remap * remove unnecessary includes * set slow down obstacle velocity to 0 * add flag to consider pointcloud obstacle for stopping & slowing down * style(pre-commit): autofix * downsample pointcloud using voxel grid * change shape type of pointcloud obstacle to polygon * convert pointcloud to obstacle by clustering * add parameters for clustering * add max_num_points parameter to dummy object * downsample pointcloud when the number of points is larger than max_num_points * add max_num_points property to dummy bus * add parameters for pointcloud based obstacles * store pointcloud in obstacle struct * change obstacle conversion method * migrate previous changes to new package * store necessary points only * move use_pointcloud to common parameter * extract necessary points from pointcloud * add use_pointcloud parameter to planner interface * fix obstacle conversion * fix collision point determination * simplify pointcloud transformation * style(pre-commit): autofix * fix collision point determination * pick nearest stop collision point * check collision for every point in cluster * migrate previous changes to new files * reduce diff * remove use_pointcloud parameter * add parameters for pointcloud filtering * add autoware namespace * Revert "add max_num_points parameter to dummy object" This reverts commit 98bcd08. * Revert "downsample pointcloud when the number of points is larger than max_num_points" This reverts commit fb00b59. * Revert "add max_num_points property to dummy bus" This reverts commit 5f9e4ab. * feat(diagnostic_graph_utils): add logging tool Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix all OK Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat(default_ad_api): add log when operation mode change fails Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * get only the necessary one of object or pointcloud data * addfield for obstacle source type * enable simultaneous use of PredictedObjects and PointCloud * separate convertToObstacles() by source type * avoid using pointer * reduce diff * make nest shallower * define vector concatenate function * shorten variable names * fix redundant condition --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <isamu.takagi@tier4.jp> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
1 parent 9836f29 commit a0e04f7

File tree

8 files changed

+404
-32
lines changed

8 files changed

+404
-32
lines changed

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

+1
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,7 @@
160160
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
161161
<remap from="~/input/acceleration" to="/localization/acceleration"/>
162162
<remap from="~/input/objects" to="/perception/object_recognition/objects"/>
163+
<remap from="~/input/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
163164
<remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
164165
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
165166
<remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>

planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

+10
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
motorcycle: true
3838
bicycle: true
3939
pedestrian: true
40+
pointcloud: false
4041

4142
cruise_obstacle_type:
4243
inside:
@@ -68,8 +69,17 @@
6869
motorcycle: true
6970
bicycle: true
7071
pedestrian: true
72+
pointcloud: false
7173

7274
behavior_determination:
75+
pointcloud_search_radius: 5.0
76+
pointcloud_voxel_grid_x: 0.05
77+
pointcloud_voxel_grid_y: 0.05
78+
pointcloud_voxel_grid_z: 100000.0
79+
pointcloud_cluster_tolerance: 1.0
80+
pointcloud_min_cluster_size: 1
81+
pointcloud_max_cluster_size: 100000
82+
7383
decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
7484

7585
prediction_resampling_time_interval: 0.1

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

+26-6
Original file line numberDiff line numberDiff line change
@@ -58,25 +58,42 @@ struct Obstacle
5858
const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance,
5959
const double lat_dist_from_obstacle_to_traj)
6060
: stamp(arg_stamp),
61+
ego_to_obstacle_distance(ego_to_obstacle_distance),
62+
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
6163
pose(arg_pose),
6264
orientation_reliable(true),
6365
twist(object.kinematics.initial_twist_with_covariance.twist),
6466
twist_reliable(true),
6567
classification(object.classification.at(0)),
6668
uuid(autoware::universe_utils::toHexString(object.object_id)),
67-
shape(object.shape),
68-
ego_to_obstacle_distance(ego_to_obstacle_distance),
69-
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj)
69+
shape(object.shape)
7070
{
7171
predicted_paths.clear();
7272
for (const auto & path : object.kinematics.predicted_paths) {
7373
predicted_paths.push_back(path);
7474
}
7575
}
7676

77-
Polygon2d toPolygon() const { return autoware::universe_utils::toPolygon2d(pose, shape); }
77+
Obstacle(
78+
const rclcpp::Time & arg_stamp,
79+
const std::optional<geometry_msgs::msg::Point> & stop_collision_point,
80+
const std::optional<geometry_msgs::msg::Point> & slow_down_front_collision_point,
81+
const std::optional<geometry_msgs::msg::Point> & slow_down_back_collision_point,
82+
const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj)
83+
: stamp(arg_stamp),
84+
ego_to_obstacle_distance(ego_to_obstacle_distance),
85+
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
86+
stop_collision_point(stop_collision_point),
87+
slow_down_front_collision_point(slow_down_front_collision_point),
88+
slow_down_back_collision_point(slow_down_back_collision_point)
89+
{
90+
}
7891

7992
rclcpp::Time stamp; // This is not the current stamp, but when the object was observed.
93+
double ego_to_obstacle_distance;
94+
double lat_dist_from_obstacle_to_traj;
95+
96+
// for PredictedObject
8097
geometry_msgs::msg::Pose pose; // interpolated with the current stamp
8198
bool orientation_reliable;
8299
Twist twist;
@@ -85,8 +102,11 @@ struct Obstacle
85102
std::string uuid;
86103
Shape shape;
87104
std::vector<PredictedPath> predicted_paths;
88-
double ego_to_obstacle_distance;
89-
double lat_dist_from_obstacle_to_traj;
105+
106+
// for PointCloud
107+
std::optional<geometry_msgs::msg::Point> stop_collision_point;
108+
std::optional<geometry_msgs::msg::Point> slow_down_front_collision_point;
109+
std::optional<geometry_msgs::msg::Point> slow_down_back_collision_point;
90110
};
91111

92112
struct TargetObstacleInterface

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp

+34-3
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@
2727
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
2828
#include <rclcpp/rclcpp.hpp>
2929

30+
#include <tf2_ros/buffer.h>
31+
#include <tf2_ros/transform_listener.h>
32+
3033
#include <algorithm>
3134
#include <memory>
3235
#include <mutex>
@@ -58,16 +61,27 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
5861
std::vector<Obstacle> convertToObstacles(
5962
const Odometry & odometry, const PredictedObjects & objects,
6063
const std::vector<TrajectoryPoint> & traj_points) const;
64+
std::vector<Obstacle> convertToObstacles(
65+
const Odometry & odometry, const PointCloud2 & pointcloud,
66+
const std::vector<TrajectoryPoint> & traj_points,
67+
const std_msgs::msg::Header & traj_header) const;
6168
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
62-
determineEgoBehaviorAgainstObstacles(
69+
determineEgoBehaviorAgainstPredictedObjectObstacles(
6370
const Odometry & odometry, const PredictedObjects & objects,
6471
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
72+
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
73+
determineEgoBehaviorAgainstPointCloudObstacles(
74+
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
75+
const std::vector<Obstacle> & obstacles);
6576
std::vector<TrajectoryPoint> decimateTrajectoryPoints(
6677
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
67-
std::optional<StopObstacle> createStopObstacle(
78+
std::optional<StopObstacle> createStopObstacleForPredictedObject(
6879
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
6980
const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
7081
const double precise_lateral_dist) const;
82+
std::optional<StopObstacle> createStopObstacleForPointCloud(
83+
const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
84+
const double precise_lateral_dist) const;
7185
bool isStopObstacle(const uint8_t label) const;
7286
bool isInsideCruiseObstacle(const uint8_t label) const;
7387
bool isOutsideCruiseObstacle(const uint8_t label) const;
@@ -94,9 +108,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
94108
double calcCollisionTimeMargin(
95109
const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
96110
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
97-
std::optional<SlowDownObstacle> createSlowDownObstacle(
111+
std::optional<SlowDownObstacle> createSlowDownObstacleForPredictedObject(
98112
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
99113
const Obstacle & obstacle, const double precise_lat_dist);
114+
std::optional<SlowDownObstacle> createSlowDownObstacleForPointCloud(
115+
const Obstacle & obstacle, const double precise_lat_dist);
100116
PlannerData createPlannerData(
101117
const Odometry & odometry, const AccelWithCovarianceStamped & acc,
102118
const std::vector<TrajectoryPoint> & traj_points) const;
@@ -116,6 +132,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
116132

117133
bool enable_debug_info_;
118134
bool enable_calculation_time_info_;
135+
bool use_pointcloud_for_stop_;
136+
bool use_pointcloud_for_slow_down_;
119137
double min_behavior_stop_margin_;
120138
bool enable_approaching_on_curve_;
121139
double additional_safe_distance_margin_on_curve_;
@@ -149,9 +167,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
149167
this, "~/input/odometry"};
150168
autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
151169
this, "~/input/objects"};
170+
autoware::universe_utils::InterProcessPollingSubscriber<PointCloud2> pointcloud_sub_{
171+
this, "~/input/pointcloud"};
152172
autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
153173
this, "~/input/acceleration"};
154174

175+
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
176+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
177+
155178
// Vehicle Parameters
156179
VehicleInfo vehicle_info_;
157180

@@ -182,6 +205,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
182205
void onParam(const std::vector<rclcpp::Parameter> & parameters);
183206

184207
double decimate_trajectory_step_length;
208+
double pointcloud_search_radius;
209+
double pointcloud_voxel_grid_x;
210+
double pointcloud_voxel_grid_y;
211+
double pointcloud_voxel_grid_z;
212+
double pointcloud_cluster_tolerance;
213+
int pointcloud_min_cluster_size;
214+
int pointcloud_max_cluster_size;
185215
// hysteresis for stop and cruise
186216
double obstacle_velocity_threshold_from_cruise_to_stop;
187217
double obstacle_velocity_threshold_from_stop_to_cruise;
@@ -271,6 +301,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
271301

272302
bool is_driving_forward_{true};
273303
bool enable_slow_down_planning_{false};
304+
bool use_pointcloud_{false};
274305

275306
std::vector<StopObstacle> prev_closest_stop_obstacles_{};
276307

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -63,12 +63,13 @@ class PlannerInterface
6363

6464
void setParam(
6565
const bool enable_debug_info, const bool enable_calculation_time_info,
66-
const double min_behavior_stop_margin, const double enable_approaching_on_curve,
67-
const double additional_safe_distance_margin_on_curve,
66+
const bool use_pointcloud, const double min_behavior_stop_margin,
67+
const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve,
6868
const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop)
6969
{
7070
enable_debug_info_ = enable_debug_info;
7171
enable_calculation_time_info_ = enable_calculation_time_info;
72+
use_pointcloud_ = use_pointcloud;
7273
min_behavior_stop_margin_ = min_behavior_stop_margin;
7374
enable_approaching_on_curve_ = enable_approaching_on_curve;
7475
additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve;
@@ -117,6 +118,7 @@ class PlannerInterface
117118
// Parameters
118119
bool enable_debug_info_{false};
119120
bool enable_calculation_time_info_{false};
121+
bool use_pointcloud_{false};
120122
LongitudinalInfo longitudinal_info_;
121123
double min_behavior_stop_margin_;
122124
bool enable_approaching_on_curve_;

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp

+7
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include "geometry_msgs/msg/transform_stamped.hpp"
2828
#include "geometry_msgs/msg/twist_stamped.hpp"
2929
#include "nav_msgs/msg/odometry.hpp"
30+
#include "sensor_msgs/msg/point_cloud2.hpp"
3031
#include "tier4_debug_msgs/msg/float32_stamped.hpp"
3132
#include "tier4_planning_msgs/msg/stop_factor.hpp"
3233
#include "tier4_planning_msgs/msg/stop_reason_array.hpp"
@@ -35,6 +36,9 @@
3536
#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp"
3637
#include "visualization_msgs/msg/marker_array.hpp"
3738

39+
#include <pcl/point_cloud.h>
40+
#include <pcl/point_types.h>
41+
3842
using autoware::vehicle_info_utils::VehicleInfo;
3943
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
4044
using autoware_adapi_v1_msgs::msg::VelocityFactor;
@@ -50,6 +54,7 @@ using geometry_msgs::msg::AccelStamped;
5054
using geometry_msgs::msg::AccelWithCovarianceStamped;
5155
using geometry_msgs::msg::Twist;
5256
using nav_msgs::msg::Odometry;
57+
using sensor_msgs::msg::PointCloud2;
5358
using tier4_debug_msgs::msg::Float32Stamped;
5459
using tier4_planning_msgs::msg::StopFactor;
5560
using tier4_planning_msgs::msg::StopReason;
@@ -63,4 +68,6 @@ namespace bg = boost::geometry;
6368
using autoware::universe_utils::Point2d;
6469
using autoware::universe_utils::Polygon2d;
6570

71+
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
72+
6673
#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_

0 commit comments

Comments
 (0)