diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 41c8b96f27ede..963c3600e6ca5 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -38,12 +38,17 @@ | `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | | `enable_z_axis_obstacle_filtering` | bool | filter obstacles in z axis (height) [-] | | `z_axis_filtering_buffer` | double | additional buffer for z axis filtering [m]] | +| `use_predicted_objects` | bool | whether to use predicted objects for collision and slowdown detection [-]] | ## Obstacle Stop Planner ### Role -This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`(center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.) +This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum +of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`( +center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as +following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points +for reducing computational costs.)
![example](./docs/collision_parameters.svg){width=1000} @@ -55,20 +60,29 @@ This module inserts the stop point before the obstacle with margin. In nominal c
target for obstacle stop planner
-If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.) +If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum +of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For +example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.)
![example](./docs/min_longitudinal_margin.svg){width=1000}
minimum longitudinal margin
-The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. +The module searches the obstacle pointcloud within detection area. When the pointcloud is +found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not +insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. ### Restart prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control +performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to +moving in order to approach the near stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle +is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle +has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is +stopped due to other factors.
![example](./docs/restart_prevention.svg){width=1000} @@ -141,7 +155,10 @@ stop ### Role -This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation. +This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward +margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum +of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The +velocity is calculated the following equation. $v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$ @@ -152,7 +169,8 @@ $v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} - $l_{margin}$ : `lateral_margin` [m] - $l_{vw}$ : width of the ego footprint [m] -The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section. +The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow +down section.
![example](./docs/slow_down_parameters.svg){width=1000} @@ -218,40 +236,42 @@ stop ### Role -`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car). - -| Parameter | Type | Description | -| ---------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------- | -| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not | -| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not | -| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | -| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | -| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | -| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | -| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | -| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | -| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | -| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | -| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | -| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | -| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | -| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | -| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | -| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | -| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | -| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | -| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | -| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | -| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | -| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | -| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | -| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | -| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | -| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | -| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | -| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | -| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed | -| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | +`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the +trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of +the front car), and the distance to the point cloud (= distance to the front car). + +| Parameter | Type | Description | +| ---------------------------------------------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------- | +| `adaptive_cruise_control.use_object_to_estimate_vel` | bool | use dynamic objects for estimating object velocity or not (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.use_pcl_to_estimate_vel` | bool | use raw pointclouds for estimating object velocity or not (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.consider_obj_velocity` | bool | consider forward vehicle velocity to calculate target velocity in adaptive cruise or not | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_start_acc` | double | start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] | +| `adaptive_cruise_control.obstacle_velocity_thresh_to_stop_acc` | double | stop acc when the velocity of the forward obstacle falls below this value [m/s] | +| `adaptive_cruise_control.emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.emergency_stop_idling_time` | double | supposed idling time to start emergency stop [s] | +| `adaptive_cruise_control.min_dist_stop` | double | minimum distance of emergency stop [m] | +| `adaptive_cruise_control.obstacle_emergency_stop_acceleration` | double | supposed minimum acceleration (deceleration) in emergency stop [m/ss] | +| `adaptive_cruise_control.max_standard_acceleration` | double | supposed maximum acceleration in active cruise control [m/ss] | +| `adaptive_cruise_control.min_standard_acceleration` | double | supposed minimum acceleration (deceleration) in active cruise control [m/ss] | +| `adaptive_cruise_control.standard_idling_time` | double | supposed idling time to react object in active cruise control [s] | +| `adaptive_cruise_control.min_dist_standard` | double | minimum distance in active cruise control [m] | +| `adaptive_cruise_control.obstacle_min_standard_acceleration` | double | supposed minimum acceleration of forward obstacle [m/ss] | +| `adaptive_cruise_control.margin_rate_to_change_vel` | double | rate of margin distance to insert target velocity [-] | +| `adaptive_cruise_control.use_time_compensation_to_calc_distance` | bool | use time-compensation to calculate distance to forward vehicle | +| `adaptive_cruise_control.p_coefficient_positive` | double | coefficient P in PID control (used when target dist -current_dist >=0) [-] | +| `adaptive_cruise_control.p_coefficient_negative` | double | coefficient P in PID control (used when target dist -current_dist <0) [-] | +| `adaptive_cruise_control.d_coefficient_positive` | double | coefficient D in PID control (used when delta_dist >=0) [-] | +| `adaptive_cruise_control.d_coefficient_negative` | double | coefficient D in PID control (used when delta_dist <0) [-] | +| `adaptive_cruise_control.object_polygon_length_margin` | double | The distance to extend the polygon length the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.object_polygon_width_margin` | double | The distance to extend the polygon width the object in pointcloud-object matching [m] | +| `adaptive_cruise_control.valid_estimated_vel_diff_time` | double | Maximum time difference treated as continuous points in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_vel_que_time` | double | Time width of information used for speed estimation in speed estimation using a point cloud [s] | +| `adaptive_cruise_control.valid_estimated_vel_max` | double | Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.valid_estimated_vel_min` | double | Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] | +| `adaptive_cruise_control.thresh_vel_to_stop` | double | Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] | +| `adaptive_cruise_control.lowpass_gain_of_upper_velocity` | double | Lowpass-gain of target velocity | +| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed (valid only if osp.use_predicted_objects false) | +| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | ### Flowchart @@ -317,37 +337,61 @@ stop This module works only when the target point is found in the detection area of the `Obstacle stop planner` module. -The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure. -If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity. -Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used. +The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses +the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. +The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the +perception failure. +If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the +dynamic object is used as the target point velocity. +Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; +that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the +target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the +estimation, the median of the calculation result for several steps is used. If the calculated velocity is within the threshold range, it is used as the target point velocity. -Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated. +Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, +the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode +transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, +and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated +distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity +to insert is calculated. The emergency distance $d\_{emergency}$ is calculated as follows. -$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ +$d_{emergency} = d_{margin_{emergency}} + t_{idling_{emergency}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ +{emergency}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{emergency}}})$ -- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends on the parameter `min_dist_stop` -- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the parameter `emergency_stop_idling_time` +- $d_{margin_{emergency}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{emergency}}$ depends + on the parameter `min_dist_stop` +- $t_{idling_{emergency}}$ is a supposed idling time. The value of $t_{idling_{emergency}}$ depends on the + parameter `emergency_stop_idling_time` - $v_{ego}$ is a current velocity of own vehicle -- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{emergency}}}$ depends on the parameter `emergency_stop_acceleration` +- $a_{ego_{_{emergency}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_ + {emergency}}}$ depends on the parameter `emergency_stop_acceleration` - $v_{obj}$ is a current velocity of obstacle pointcloud. -- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` +- $a_{obj_{_{emergency}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_ + {emergency}}}$ depends on the parameter `obstacle_emergency_stop_acceleration` - \*Above $X_{_{emergency}}$ parameters are used only in emergency situation. -The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard distance $d\_{standard}$ calculated as following. Therefore, if the distance -to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current velocity, and vice versa. For keeping the distance, a PID controller is used. +The target velocity is determined to keep the distance to the obstacle pointcloud from own vehicle at the standard +distance $d\_{standard}$ calculated as following. Therefore, if the distance +to the obstacle pointcloud is longer than standard distance, The target velocity becomes higher than the current +velocity, and vice versa. For keeping the distance, a PID controller is used. -$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ +$d_{standard} = d_{margin_{standard}} + t_{idling_{standard}} \cdot v_{ego} + (-\frac{v_{ego}^2}{2 \cdot a_{ego_ +{standard}}}) - (-\frac{v_{obj}^2}{2 \cdot a_{obj_{standard}}})$ -- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends on the parameter `min_dist_stop` -- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the parameter `standard_stop_idling_time` +- $d_{margin_{standard}}$ is a minimum margin to the obstacle pointcloud. The value of $d_{margin_{standard}}$ depends + on the parameter `min_dist_stop` +- $t_{idling_{standard}}$ is a supposed idling time. The value of $t_{idling_{standard}}$ depends on the + parameter `standard_stop_idling_time` - $v_{ego}$ is a current velocity of own vehicle -- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_{standard}}}$ depends on the parameter `min_standard_acceleration` +- $a_{ego_{_{standard}}}$ is a minimum acceleration (maximum deceleration) of own vehicle. The value of $a_{ego_{_ + {standard}}}$ depends on the parameter `min_standard_acceleration` - $v_{obj}$ is a current velocity of obstacle pointcloud. -- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_{standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` +- $a_{obj_{_{standard}}}$ is a supposed minimum acceleration of obstacle pointcloud. The value of $a_{obj_{_ + {standard}}}$ depends on the parameter `obstacle_min_standard_acceleration` - \*Above $X_{_{standard}}$ parameters are used only in non-emergency situation. ![adaptive_cruise](./docs/adaptive_cruise.drawio.svg) @@ -356,8 +400,14 @@ If the target velocity exceeds the value of `thresh_vel_to_stop`, the target vel ## Known Limits -- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's behavior as much as possible and always be ready for overriding. +- It is strongly depends on velocity planning module whether or not it moves according to the target speed embedded + by `Adaptive Cruise Controller` module. If the velocity planning module is updated, please take care of the vehicle's + behavior as much as possible and always be ready for overriding. -- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves dangerously. +- The velocity estimation algorithm in `Adaptive Cruise Controller` is depend on object tracking module. Please note + that if the object-tracking fails or the tracking result is incorrect, it the possibility that the vehicle behaves + dangerously. -- It does not work for backward driving, but publishes the path of the input as it is. Please use [obstacle_cruise_planner](../obstacle_cruise_planner/README.md) if you want to stop against an obstacle when backward driving. +- It does not work for backward driving, but publishes the path of the input as it is. Please + use [obstacle_cruise_planner](../obstacle_cruise_planner/README.md) if you want to stop against an obstacle when + backward driving. diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index b93faad3a4131..40a3f36bff903 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include +#include #include #include @@ -32,6 +33,7 @@ namespace motion_planning { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -47,6 +49,14 @@ class AdaptiveCruiseController const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); + void insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header, + const PredictedObject & target_object); + private: rclcpp::Publisher::SharedPtr pub_debug_; @@ -187,6 +197,8 @@ class AdaptiveCruiseController bool estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, double * velocity); + void calculateProjectedVelocityFromObject( + const PredictedObject & object, const double traj_yaw, double * velocity); double estimateRoughPointVelocity(double current_vel); bool isObstacleVelocityHigh(const double obj_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 26db4debbeeb5..0acfa2b8f067a 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -88,7 +88,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; - +using autoware_auto_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) @@ -102,13 +102,15 @@ struct ObstacleWithDetectionTime struct PredictedObjectWithDetectionTime { - explicit PredictedObjectWithDetectionTime(const rclcpp::Time & t, Pose & p) - : detection_time(t), point(p) + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) { } rclcpp::Time detection_time; - Pose point; + geometry_msgs::msg::Point point; + PredictedObject object; }; class ObstacleStopPlannerNode : public rclcpp::Node @@ -173,8 +175,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); void searchPredictedObject( - const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data, - const VehicleInfo & vehicle_info, const StopParam & stop_param); + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param); void insertVelocity( TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header, diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 1835c33b9a65b..0af3a1eb96279 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -249,6 +250,8 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; + autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; + Pose nearest_collision_point_pose{}; Pose nearest_slow_down_point_pose{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 75853b9f9ab40..b28739cd9d073 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include +#include #include #include @@ -54,6 +56,9 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using PointVariant = boost::variant; boost::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, @@ -101,12 +106,12 @@ void getLateralNearestPoint( const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation); -void getNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point, - rclcpp::Time * nearest_collision_point_time); +double getNearestPointAndDistanceForPredictedObject( + const geometry_msgs::msg::PoseArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); void getLateralNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point, + const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation); Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); @@ -137,6 +142,13 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e TrajectoryPoint getExtendTrajectoryPoint( double extend_distance, const TrajectoryPoint & goal_point); +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +pcl::PointXYZ pointToPcl(const double x, const double y, const double z); + +boost::optional getObstacleFromUuid( + const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id); + rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 83c82c602c93f..22041c7b59724 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -273,6 +273,68 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( *need_to_stop = false; } +void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( + const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, + const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const rclcpp::Time nearest_collision_point_time, + const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, + TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header, + const PredictedObject & target_object) +{ + debug_values_.data.clear(); + debug_values_.data.resize(num_debug_values_, 0.0); + + const double current_velocity = current_odometry_ptr->twist.twist.linear.x; + double col_point_distance; + double point_velocity; + /* + * calc distance to collision point + */ + calcDistanceToNearestPointOnPath( + trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, + nearest_collision_point_time, &col_point_distance, trajectory_header); + + /* + * calc yaw of trajectory at collision point + */ + const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx); + + /* + * estimate velocity of collision point + */ + calculateProjectedVelocityFromObject(target_object, traj_yaw, &point_velocity); + + // calculate max(target) velocity of self + const double upper_velocity = + calcUpperVelocity(col_point_distance, point_velocity, current_velocity); + pub_debug_->publish(debug_values_); + + if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if the target object is obstacle return stop true + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Target object is pedestrian. Insert stop line."); + *need_to_stop = true; + return; + } + + if (upper_velocity <= param_.thresh_vel_to_stop) { + // if upper velocity is too low, need to stop + RCLCPP_DEBUG_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "Upper velocity is too low. Insert stop line."); + *need_to_stop = true; + return; + } + + /* + * insert max velocity + */ + insertMaxVelocityToPath( + self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); + *need_to_stop = false; +} + void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const TrajectoryPoints & trajectory, const int nearest_point_idx, const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, @@ -376,6 +438,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( } } +void AdaptiveCruiseController::calculateProjectedVelocityFromObject( + const PredictedObject & object, const double traj_yaw, double * velocity) +{ + /* get object velocity, and current yaw */ + double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + + *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; +} + bool AdaptiveCruiseController::estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, double * velocity) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 88984c833caf9..84571085dc61c 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -41,6 +41,7 @@ namespace motion_planning { +using autoware_auto_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -192,10 +193,13 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod this->create_publisher("~/debug/collision_pointcloud", 1); // Subscribers - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), - createSubscriptionOptions(this)); + if (!node_param_.use_predicted_objects) { + // No need to point cloud while using predicted objects + sub_point_cloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), + createSubscriptionOptions(this)); + } sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, @@ -273,7 +277,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m return; } - if (!obstacle_ros_pointcloud_ptr) { + if (!obstacle_ros_pointcloud_ptr && !node_param_.use_predicted_objects) { waiting("obstacle pointcloud"); return; } @@ -328,7 +332,9 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m base_trajectory, stop_param.step_length, planner_data.decimate_trajectory_index_map); if (node_param_.use_predicted_objects) { - searchPredictedObject(decimate_trajectory, planner_data, vehicle_info, stop_param); + searchPredictedObject( + decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info, + stop_param); } else { // search obstacles within slow-down/collision area searchObstacle( @@ -568,13 +574,16 @@ void ObstacleStopPlannerNode::searchObstacle( } void ObstacleStopPlannerNode::searchPredictedObject( - const TrajectoryPoints & decimate_trajectory, PlannerData & planner_data, - const VehicleInfo & vehicle_info, const StopParam & stop_param) + const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param) { + mutex_.lock(); const auto object_ptr = object_ptr_; - if (object_ptr->objects.empty()) { - return; - } + mutex_.unlock(); + + // TODO(brkay54): Add filtering function here! + const auto now = this->now(); updatePredictedObstacleHistory(now); @@ -582,458 +591,387 @@ void ObstacleStopPlannerNode::searchPredictedObject( // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; const auto & p_back = decimate_trajectory.at(i + 1).pose; - const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); - const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); - const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + const auto z_axis_min = p_front.position.z; + const auto z_axis_max = + p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer; if (node_param_.enable_slow_down) { - for (const auto & obj : object_ptr->objects) { + double min_slow_down_norm = 0.0; + bool is_init = false; + size_t nearest_slow_down_object_index = 0; + geometry_msgs::msg::Point nearest_slow_down_point; + geometry_msgs::msg::PoseArray slow_down_points; + + for (size_t j = 0; j < object_ptr->objects.size(); ++j) { + const auto & obj = object_ptr->objects.at(j); + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Polygon2d one_step_move_slow_down_range; + bool found_slow_down_object = false; + Polygon2d object_polygon{}; if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - - Polygon2d one_step_move_slow_down_range_polygon; - // create one step polygon for slow_down range createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, + p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - planner_data.found_slow_down_points = - bg::intersects(one_step_move_slow_down_range_polygon, object_polygon); - - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection( - one_step_move_slow_down_range_polygon, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); - } - - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); - - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); - } + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range, vehicle_info, + slow_down_param_.vehicle_lateral_margin); const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - Polygon2d one_step_move_slow_down_range_polygon; - // create one step polygon for slow_down range + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); + + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.vehicle_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - - planner_data.found_slow_down_points = - bg::intersects(one_step_move_slow_down_range_polygon, object_polygon); - - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection( - one_step_move_slow_down_range_polygon, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); - } + p_front, p_back, one_step_move_slow_down_range, vehicle_info, + slow_down_param_.unknown_lateral_margin); - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); + object_polygon = convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); + found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (found_slow_down_object) { + geometry_msgs::msg::PoseArray slow_down_points_tmp; + + std::vector slow_down_point; + bg::intersection(one_step_move_slow_down_range, object_polygon, slow_down_point); + for (const auto & point : slow_down_point) { + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + slow_down_points_tmp.poses.push_back(pose); } - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; - object_polygon = convertPolygonObjectToGeometryPolygon( - obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - Polygon2d one_step_move_slow_down_range_polygon; - // create one step polygon for slow_down range - createOneStepPolygon( - p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); - planner_data.found_slow_down_points = - bg::intersects(one_step_move_slow_down_range_polygon, object_polygon); - const auto found_first_slow_down_points = - planner_data.found_slow_down_points && !planner_data.slow_down_require; - - if (found_first_slow_down_points) { - // found nearest slow down obstacle - planner_data.decimate_trajectory_slow_down_index = i; - planner_data.slow_down_require = true; - std::vector slow_down_point; - geometry_msgs::msg::PoseArray slow_down_points; - bg::intersection( - one_step_move_slow_down_range_polygon, object_polygon, slow_down_point); - for (const auto & point : slow_down_point) { + // Also check the corner points + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_slow_down_range)) { geometry_msgs::msg::Pose pose; pose.position.x = point.x(); pose.position.y = point.y(); - slow_down_points.poses.push_back(pose); + slow_down_points_tmp.poses.push_back(pose); } + } + geometry_msgs::msg::Point nearest_slow_down_point_tmp; + const double norm = getNearestPointAndDistanceForPredictedObject( + slow_down_points_tmp, p_front, &nearest_slow_down_point_tmp); + if (norm < min_slow_down_norm || !is_init) { + min_slow_down_norm = norm; + nearest_slow_down_point = nearest_slow_down_point_tmp; + is_init = true; + nearest_slow_down_object_index = j; + slow_down_points = slow_down_points_tmp; + } + } + } - getNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.nearest_slow_down_point_pose, - &planner_data.nearest_collision_point_time); + planner_data.found_slow_down_points = is_init; - getLateralNearestPointForPredictedObject( - slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point_pose, - &planner_data.lateral_deviation); + const auto found_first_slow_down_points = + planner_data.found_slow_down_points && !planner_data.slow_down_require; - debug_ptr_->pushObstaclePoint( - planner_data.nearest_slow_down_point_pose.position, PointType::SlowDown); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); - } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + if (found_first_slow_down_points) { + // found nearest slow down obstacle + planner_data.decimate_trajectory_slow_down_index = i; + planner_data.slow_down_require = true; + planner_data.nearest_slow_down_point = + pointToPcl(nearest_slow_down_point.x, nearest_slow_down_point.y, p_front.position.z); + planner_data.nearest_collision_point_time = object_ptr->header.stamp; + planner_data.slow_down_object_shape = + object_ptr->objects.at(nearest_slow_down_object_index).shape; + + // TODO(brkay54): lateral_nearest_slow_down_point_pose and nearest_slow_down_point_pose are + // not used + getLateralNearestPointForPredictedObject( + slow_down_points, p_front, &planner_data.lateral_nearest_slow_down_point, + &planner_data.lateral_deviation); + + // Push slow down debugging points + Polygon2d one_step_move_slow_down_vehicle_polygon; + + const auto & obj = object_ptr->objects.at(nearest_slow_down_object_index); + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.pedestrian_lateral_margin); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.vehicle_lateral_margin); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, + slow_down_param_.unknown_lateral_margin); } + debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); + + debug_ptr_->pushPolygon( + one_step_move_slow_down_vehicle_polygon, p_front.position.z, PolygonType::SlowDown); + } else { + // only used for pedestrian and debugging + Polygon2d one_step_move_slow_down_range_dbg; + createOneStepPolygon( + p_front, p_back, one_step_move_slow_down_range_dbg, vehicle_info, + slow_down_param_.pedestrian_lateral_margin); + + debug_ptr_->pushPolygon( + one_step_move_slow_down_range_dbg, p_front.position.z, PolygonType::SlowDownRange); } } { - for (const auto & obj : object_ptr->objects) { + double min_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + geometry_msgs::msg::Point nearest_collision_point; + + for (size_t j = 0; j < object_ptr->objects.size(); ++j) { + const auto & obj = object_ptr->objects.at(j); + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Polygon2d one_step_move_collision_polygon; + bool found_collision_object = false; + Polygon2d object_polygon{}; if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); - - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; - } + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.vehicle_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); - - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; - - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; - } + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + p_front, p_back, one_step_move_collision_polygon, vehicle_info, stop_param.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - const auto found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); + found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (found_collision_object) { + geometry_msgs::msg::PoseArray collision_points_tmp; - if (found_collision_points) { - Pose nearest_collision_point_pose; - rclcpp::Time nearest_collision_point_time; + std::vector collision_point; + bg::intersection(one_step_move_collision_polygon, object_polygon, collision_point); + for (const auto & point : collision_point) { + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + collision_points_tmp.poses.push_back(pose); + } - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { + // Also check the corner points + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_collision_polygon)) { geometry_msgs::msg::Pose pose; pose.position.x = point.x(); pose.position.y = point.y(); - collision_points.poses.push_back(pose); + collision_points_tmp.poses.push_back(pose); } - getNearestPointForPredictedObject( - collision_points, p_front, &nearest_collision_point_pose, - &nearest_collision_point_time); - - predicted_object_history_.emplace_back(now, nearest_collision_point_pose); - break; } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + geometry_msgs::msg::Point nearest_collision_point_tmp; + const double norm = getNearestPointAndDistanceForPredictedObject( + collision_points_tmp, p_front, &nearest_collision_point_tmp); + if (norm < min_collision_norm || !is_init) { + min_collision_norm = norm; + nearest_collision_point = nearest_collision_point_tmp; + is_init = true; + nearest_collision_object_index = j; + } } } + if (is_init) { + predicted_object_history_.emplace_back( + now, nearest_collision_point, object_ptr->objects.at(nearest_collision_object_index)); + break; + } + + // only used for pedestrian + Polygon2d one_step_move_collision_dbg; + createOneStepPolygon( + p_front, p_back, one_step_move_collision_dbg, vehicle_info, + stop_param.pedestrian_lateral_margin); + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_collision_dbg, z_axis_min, z_axis_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_collision_dbg, p_front.position.z, PolygonType::Vehicle); + } } } + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + if (predicted_object_history_.empty()) { + break; + } + // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; const auto & p_back = decimate_trajectory.at(i + 1).pose; - const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); - const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); - const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); - for (const auto & obj : object_ptr->objects) { - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - Polygon2d object_polygon{}; - object_polygon = convertCylindricalObjectToGeometryPolygon( - obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + const auto z_axis_min = p_front.position.z; + const auto z_axis_max = + p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer; + + double min_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle + for (size_t j = 0; j < predicted_object_history_.size(); ++j) { + // check new collision points + const auto & obj = predicted_object_history_.at(j).object; + if (node_param_.enable_z_axis_obstacle_filtering) { + if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) { + continue; + } + } + Point2d collision_point; + collision_point.x() = predicted_object_history_.at(j).point.x; + collision_point.y() = predicted_object_history_.at(j).point.y; + Polygon2d one_step_move_vehicle_polygon; + // create one step polygon for vehicle + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.vehicle_lateral_margin); - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.unknown_lateral_margin); - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle); - } - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } - break; + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + continue; + } + if (bg::within(collision_point, one_step_move_vehicle_polygon)) { + const double norm = calcDistance2d(predicted_object_history_.at(j).point, p_front.position); + if (norm < min_collision_norm || !is_init) { + min_collision_norm = norm; + is_init = true; + nearest_collision_object_index = j; } + } + } + + planner_data.found_collision_points = is_init; + + if (planner_data.found_collision_points) { + planner_data.nearest_collision_point = pointToPcl( + predicted_object_history_.at(nearest_collision_object_index).point.x, + predicted_object_history_.at(nearest_collision_object_index).point.y, p_front.position.z); + + planner_data.decimate_trajectory_collision_index = i; + + planner_data.nearest_collision_point_time = + predicted_object_history_.at(nearest_collision_object_index).detection_time; + + // create one step polygon for vehicle collision debug + Polygon2d one_step_move_vehicle_polygon; + Polygon2d object_polygon{}; + + const auto & obj = predicted_object_history_.at(nearest_collision_object_index).object; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.pedestrian_lateral_margin); + object_polygon = convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, + stop_param.vehicle_lateral_margin); const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - Polygon2d object_polygon{}; object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle + + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.unknown_lateral_margin); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); - - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; - - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); - - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle); - } - - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } - break; - } - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - Polygon2d object_polygon{}; object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - Polygon2d one_step_move_vehicle_polygon; - // create one step polygon for vehicle - createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, - stop_param.vehicle_lateral_margin); + if (node_param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Collision); + } else { debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - // check new collision points - planner_data.found_collision_points = - bg::intersects(one_step_move_vehicle_polygon, object_polygon); + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + } - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - rclcpp::Time nearest_collision_point_time; + if (node_param_.publish_obstacle_polygon) { + debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle); + } - std::vector collision_point; - geometry_msgs::msg::PoseArray collision_points; - bg::intersection(one_step_move_vehicle_polygon, object_polygon, collision_point); - for (const auto & point : collision_point) { - geometry_msgs::msg::Pose pose; - pose.position.x = point.x(); - pose.position.y = point.y(); - collision_points.poses.push_back(pose); - } - getNearestPointForPredictedObject( - collision_points, p_front, &planner_data.nearest_collision_point_pose, - &nearest_collision_point_time); + planner_data.stop_require = planner_data.found_collision_points; + mutex_.lock(); + const auto current_odometry_ptr = current_odometry_ptr_; + const auto latest_object_ptr = object_ptr_; + mutex_.unlock(); + // find latest state of predicted object + auto obj_latest_state = getObstacleFromUuid(*latest_object_ptr, obj.object_id); + if (!obj_latest_state) { + // Can not find the object in the latest object list. Send previous state. + obj_latest_state = boost::make_optional(obj); + } - debug_ptr_->pushObstaclePoint( - planner_data.nearest_collision_point_pose.position, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - one_step_move_vehicle_polygon.clear(); - if (node_param_.publish_obstacle_polygon) { - debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle); - } - planner_data.stop_require = planner_data.found_collision_points; - mutex_.lock(); - const auto current_odometry_ptr = current_odometry_ptr_; - mutex_.unlock(); - if (!planner_data.stop_require) { - predicted_object_history_.clear(); - } + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, current_odometry_ptr, &planner_data.stop_require, + &output, trajectory_header, *obj_latest_state); - break; - } - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); + if (!planner_data.stop_require) { + predicted_object_history_.clear(); } + break; } } } @@ -1052,18 +990,11 @@ void ObstacleStopPlannerNode::insertVelocity( planner_data.decimate_trajectory_collision_index) + planner_data.trajectory_trim_index; boost::optional> index_with_dist_remain; - if (node_param_.use_predicted_objects) { - index_with_dist_remain = findNearestFrontIndex( - std::min(idx, traj_end_idx), output, - createPoint( - planner_data.nearest_collision_point_pose.position.x, - planner_data.nearest_collision_point_pose.position.y, 0)); - } else { - index_with_dist_remain = findNearestFrontIndex( - std::min(idx, traj_end_idx), output, - createPoint( - planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); - } + + index_with_dist_remain = findNearestFrontIndex( + std::min(idx, traj_end_idx), output, + createPoint( + planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0)); if (index_with_dist_remain) { const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx); @@ -1186,30 +1117,34 @@ void ObstacleStopPlannerNode::insertVelocity( double slow_down_velocity; if (node_param_.use_predicted_objects) { - for (const auto & obj : object_ptr_->objects) { - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.pedestrian_lateral_margin; - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.vehicle_lateral_margin; - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - slow_down_velocity = - slow_down_param_.min_slow_down_velocity + - (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * - std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.unknown_lateral_margin; - } else { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", - obj.shape.type); - } + if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.pedestrian_lateral_margin; + } else if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.vehicle_lateral_margin; + } else if ( + planner_data.slow_down_object_shape.type == + autoware_auto_perception_msgs::msg::Shape::POLYGON) { + slow_down_velocity = + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * + std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / + slow_down_param_.unknown_lateral_margin; + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d", + planner_data.slow_down_object_shape.type); } } else { slow_down_velocity = diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index cf36795a478c2..646fbba0022c0 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -27,6 +27,8 @@ namespace motion_planning { +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -467,6 +469,31 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e return output; } +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +pcl::PointXYZ pointToPcl(const double x, const double y, const double z) +{ + // Store the point components in a variant + PointVariant x_variant = x; + PointVariant y_variant = y; + PointVariant z_variant = z; + + // Extract the corresponding components from the variant + auto extract_float = [](const auto & variant) { return boost::get(variant); }; + float pcl_x = boost::apply_visitor(extract_float, x_variant); + float pcl_y = boost::apply_visitor(extract_float, y_variant); + float pcl_z = boost::apply_visitor(extract_float, z_variant); + + // Create a new pcl::PointXYZ object + return {pcl_x, pcl_y, pcl_z}; +} + bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose) { try { @@ -527,34 +554,34 @@ void getLateralNearestPoint( *deviation = min_norm; } -void getNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * nearest_collision_point, - rclcpp::Time * nearest_collision_point_time) +double getNearestPointAndDistanceForPredictedObject( + const geometry_msgs::msg::PoseArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) { double min_norm = 0.0; bool is_init = false; - for (const auto & p : object.poses) { - double norm = calcDistance2d(p, base_pose); + for (const auto & p : points.poses) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); if (norm < min_norm || !is_init) { min_norm = norm; - *nearest_collision_point = p; - *nearest_collision_point_time = (object.header).stamp; + *nearest_collision_point = p.position; is_init = true; } } + return min_norm; } void getLateralNearestPointForPredictedObject( - const PoseArray & object, const Pose & base_pose, Pose * lateral_nearest_point, + const PoseArray & object, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, double * deviation) { double min_norm = std::numeric_limits::max(); - for (size_t i = 0; i < object.poses.size(); ++i) { - double norm = calcDistance2d(object.poses.at(i), base_pose); + for (const auto & pose : object.poses) { + double norm = calcDistance2d(pose, base_pose); if (norm < min_norm) { min_norm = norm; - *lateral_nearest_point = object.poses.at(i); + *lateral_nearest_point = pointToPcl(pose.position.x, pose.position.y, base_pose.position.z); } } *deviation = min_norm; @@ -658,4 +685,19 @@ Polygon2d convertPolygonObjectToGeometryPolygon( : tier4_autoware_utils::inverseClockwise(object_polygon); return object_polygon; } + +boost::optional getObstacleFromUuid( + const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id) +{ + const auto itr = std::find_if( + obstacles.objects.begin(), obstacles.objects.end(), [&](PredictedObject predicted_object) { + return predicted_object.object_id == target_object_id; + }); + + if (itr == obstacles.objects.end()) { + return boost::none; + } + return boost::make_optional(*itr); +} + } // namespace motion_planning