Skip to content

Commit 6b884ea

Browse files
feat(obstacle_cruise_planner): yield function for ocp (#6242)
* WIP make ego yield to cut in vehicles Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * param name change Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> * add params Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * refactor Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> * fix bug for distance comparison Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> * implement a collision time check for yield candidates Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> * deleting comments Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp> * comment out prints for test Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * delete comments, use param Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Add param to set moving obstacle speed threshold Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * WIP check for cars going opposite direction Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * check opposite line driving Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update documentation Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update README Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * set default false Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * refactor Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * refactor isFrontObstacle Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * make const Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
1 parent 9b4d7e4 commit 6b884ea

File tree

5 files changed

+225
-21
lines changed

5 files changed

+225
-21
lines changed

planning/obstacle_cruise_planner/README.md

+14
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,20 @@ The obstacles meeting the following condition are determined as obstacles for cr
108108
| `behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold` | double | maximum obstacle velocity for cruise obstacle outside the trajectory |
109109
| `behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold` | double | maximum overlap time of the collision between the ego and obstacle |
110110

111+
##### Yield for vehicles that might cut in into the ego's lane
112+
113+
It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane.
114+
115+
The obstacles meeting the following condition are determined as obstacles for yielding (cruising).
116+
117+
- The object type is for cruising according to `common.cruise_obstacle_type.*` and it is moving with a speed greater than `behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold`.
118+
- The object is not crossing the ego's trajectory (\*1).
119+
- There is another object of type `common.cruise_obstacle_type.*` stopped in front of the moving obstacle.
120+
- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `behavior_determination.cruise.yield.max_lat_dist_between_obstacles`
121+
- Both obstacles, moving and stopped, are within `behavior_determination.cruise.yield.lat_distance_threshold` and `behavior_determination.cruise.yield.lat_distance_threshold` + `behavior_determination.cruise.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
122+
123+
If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle.
124+
111125
#### Determine stop vehicles
112126

113127
Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping.

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

+6-1
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,12 @@
9595
obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
9696
ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s]
9797
max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
98-
98+
yield:
99+
enable_yield: false
100+
lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
101+
max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
102+
max_obstacles_collision_time: 10.0 # how far the blocking obstacle
103+
stopped_obstacle_velocity_threshold: 0.5
99104
slow_down:
100105
max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width
101106
lat_hysteresis_margin: 0.2

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -55,15 +55,18 @@ struct Obstacle
5555
{
5656
Obstacle(
5757
const rclcpp::Time & arg_stamp, const PredictedObject & object,
58-
const geometry_msgs::msg::Pose & arg_pose)
58+
const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance,
59+
const double lat_dist_from_obstacle_to_traj)
5960
: stamp(arg_stamp),
6061
pose(arg_pose),
6162
orientation_reliable(true),
6263
twist(object.kinematics.initial_twist_with_covariance.twist),
6364
twist_reliable(true),
6465
classification(object.classification.at(0)),
6566
uuid(tier4_autoware_utils::toHexString(object.object_id)),
66-
shape(object.shape)
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)
6770
{
6871
predicted_paths.clear();
6972
for (const auto & path : object.kinematics.predicted_paths) {
@@ -82,6 +85,8 @@ struct Obstacle
8285
std::string uuid;
8386
Shape shape;
8487
std::vector<PredictedPath> predicted_paths;
88+
double ego_to_obstacle_distance;
89+
double lat_dist_from_obstacle_to_traj;
8590
};
8691

8792
struct TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
7070
std::optional<geometry_msgs::msg::Point> createCollisionPointForStopObstacle(
7171
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
7272
const Obstacle & obstacle) const;
73+
std::optional<CruiseObstacle> createYieldCruiseObstacle(
74+
const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points);
75+
std::optional<std::vector<CruiseObstacle>> findYieldCruiseObstacles(
76+
const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points);
7377
std::optional<CruiseObstacle> createCruiseObstacle(
7478
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
7579
const Obstacle & obstacle, const double precise_lat_dist);
@@ -196,8 +200,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
196200
int successive_num_to_entry_slow_down_condition;
197201
int successive_num_to_exit_slow_down_condition;
198202
// consideration for the current ego pose
199-
bool enable_to_consider_current_pose{false};
200203
double time_to_convergence{1.5};
204+
bool enable_to_consider_current_pose{false};
205+
// yield related parameters
206+
bool enable_yield{false};
207+
double yield_lat_distance_threshold;
208+
double max_lat_dist_between_obstacles;
209+
double stopped_obstacle_velocity_threshold;
210+
double max_obstacles_collision_time;
201211
};
202212
BehaviorDeterminationParam behavior_determination_param_;
203213

planning/obstacle_cruise_planner/src/node.cpp

+187-17
Original file line numberDiff line numberDiff line change
@@ -54,20 +54,15 @@ std::optional<T> getObstacleFromUuid(
5454
return *itr;
5555
}
5656

57-
bool isFrontObstacle(
57+
std::optional<double> calcDistanceToFrontVehicle(
5858
const std::vector<TrajectoryPoint> & traj_points, const size_t ego_idx,
5959
const geometry_msgs::msg::Point & obstacle_pos)
6060
{
6161
const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos);
62-
63-
const double ego_to_obstacle_distance =
62+
const auto ego_to_obstacle_distance =
6463
motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx);
65-
66-
if (ego_to_obstacle_distance < 0) {
67-
return false;
68-
}
69-
70-
return true;
64+
if (ego_to_obstacle_distance < 0.0) return std::nullopt;
65+
return ego_to_obstacle_distance;
7166
}
7267

7368
PredictedPath resampleHighestConfidencePredictedPath(
@@ -254,6 +249,15 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
254249
node.declare_parameter<double>("behavior_determination.stop.max_lat_margin");
255250
max_lat_margin_for_cruise =
256251
node.declare_parameter<double>("behavior_determination.cruise.max_lat_margin");
252+
enable_yield = node.declare_parameter<bool>("behavior_determination.cruise.yield.enable_yield");
253+
yield_lat_distance_threshold =
254+
node.declare_parameter<double>("behavior_determination.cruise.yield.lat_distance_threshold");
255+
max_lat_dist_between_obstacles = node.declare_parameter<double>(
256+
"behavior_determination.cruise.yield.max_lat_dist_between_obstacles");
257+
stopped_obstacle_velocity_threshold = node.declare_parameter<double>(
258+
"behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold");
259+
max_obstacles_collision_time = node.declare_parameter<double>(
260+
"behavior_determination.cruise.yield.max_obstacles_collision_time");
257261
max_lat_margin_for_slow_down =
258262
node.declare_parameter<double>("behavior_determination.slow_down.max_lat_margin");
259263
lat_hysteresis_margin_for_slow_down =
@@ -309,6 +313,20 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
309313
parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop);
310314
tier4_autoware_utils::updateParam<double>(
311315
parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise);
316+
tier4_autoware_utils::updateParam<bool>(
317+
parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield);
318+
tier4_autoware_utils::updateParam<double>(
319+
parameters, "behavior_determination.cruise.yield.lat_distance_threshold",
320+
yield_lat_distance_threshold);
321+
tier4_autoware_utils::updateParam<double>(
322+
parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles",
323+
max_lat_dist_between_obstacles);
324+
tier4_autoware_utils::updateParam<double>(
325+
parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold",
326+
stopped_obstacle_velocity_threshold);
327+
tier4_autoware_utils::updateParam<double>(
328+
parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time",
329+
max_obstacles_collision_time);
312330
tier4_autoware_utils::updateParam<double>(
313331
parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down);
314332
tier4_autoware_utils::updateParam<double>(
@@ -493,7 +511,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
493511
// (3) not too far from trajectory
494512
const auto target_obstacles = convertToObstacles(traj_points);
495513

496-
// 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
514+
// 2. Determine ego's behavior against each obstacle from stop, cruise and slow down.
497515
const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] =
498516
determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles);
499517

@@ -590,6 +608,7 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
590608
stop_watch_.tic(__func__);
591609

592610
const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp);
611+
const auto & p = behavior_determination_param_;
593612

594613
std::vector<Obstacle> target_obstacles;
595614
for (const auto & predicted_object : objects_ptr_->objects) {
@@ -611,24 +630,25 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
611630

612631
// 2. Check if the obstacle is in front of the ego.
613632
const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, ego_odom_ptr_->pose.pose);
614-
const bool is_front_obstacle =
615-
isFrontObstacle(traj_points, ego_idx, current_obstacle_pose.pose.position);
616-
if (!is_front_obstacle) {
633+
const auto ego_to_obstacle_distance =
634+
calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position);
635+
if (!ego_to_obstacle_distance) {
617636
RCLCPP_INFO_EXPRESSION(
618637
get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.",
619638
object_id.c_str());
620639
continue;
621640
}
622641

623642
// 3. Check if rough lateral distance is smaller than the threshold
643+
const double lat_dist_from_obstacle_to_traj =
644+
motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);
645+
624646
const double min_lat_dist_to_traj_poly = [&]() {
625-
const double lat_dist_from_obstacle_to_traj =
626-
motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);
627647
const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape);
628648
return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m -
629649
obstacle_max_length;
630650
}();
631-
const auto & p = behavior_determination_param_;
651+
632652
const double max_lat_margin = std::max(
633653
std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise),
634654
p.max_lat_margin_for_slow_down);
@@ -639,7 +659,9 @@ std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
639659
continue;
640660
}
641661

642-
const auto target_obstacle = Obstacle(obj_stamp, predicted_object, current_obstacle_pose.pose);
662+
const auto target_obstacle = Obstacle(
663+
obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(),
664+
lat_dist_from_obstacle_to_traj);
643665
target_obstacles.push_back(target_obstacle);
644666
}
645667

@@ -741,6 +763,23 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
741763
continue;
742764
}
743765
}
766+
const auto & p = behavior_determination_param_;
767+
if (p.enable_yield) {
768+
const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points);
769+
if (yield_obstacles) {
770+
for (const auto & y : yield_obstacles.value()) {
771+
// Check if there is no member with the same UUID in cruise_obstacles
772+
auto it = std::find_if(
773+
cruise_obstacles.begin(), cruise_obstacles.end(),
774+
[&y](const auto & c) { return y.uuid == c.uuid; });
775+
776+
// If no matching UUID found, insert yield obstacle into cruise_obstacles
777+
if (it == cruise_obstacles.end()) {
778+
cruise_obstacles.push_back(y);
779+
}
780+
}
781+
}
782+
}
744783
slow_down_condition_counter_.removeCounterUnlessUpdated();
745784

746785
// Check target obstacles' consistency
@@ -829,6 +868,137 @@ std::optional<CruiseObstacle> ObstacleCruisePlannerNode::createCruiseObstacle(
829868
tangent_vel, normal_vel, *collision_points};
830869
}
831870

871+
std::optional<CruiseObstacle> ObstacleCruisePlannerNode::createYieldCruiseObstacle(
872+
const Obstacle & obstacle, const std::vector<TrajectoryPoint> & traj_points)
873+
{
874+
if (traj_points.empty()) return std::nullopt;
875+
// check label
876+
const auto & object_id = obstacle.uuid.substr(0, 4);
877+
const auto & p = behavior_determination_param_;
878+
879+
if (!isOutsideCruiseObstacle(obstacle.classification.label)) {
880+
RCLCPP_INFO_EXPRESSION(
881+
get_logger(), enable_debug_info_,
882+
"[Cruise] Ignore yield obstacle (%s) since its type is not designated.", object_id.c_str());
883+
return std::nullopt;
884+
}
885+
886+
if (
887+
std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) <
888+
p.outside_obstacle_min_velocity_threshold) {
889+
RCLCPP_INFO_EXPRESSION(
890+
get_logger(), enable_debug_info_,
891+
"[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", object_id.c_str());
892+
return std::nullopt;
893+
}
894+
895+
if (isObstacleCrossing(traj_points, obstacle)) {
896+
RCLCPP_INFO_EXPRESSION(
897+
get_logger(), enable_debug_info_,
898+
"[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..",
899+
object_id.c_str());
900+
return std::nullopt;
901+
}
902+
903+
const auto collision_points = [&]() -> std::optional<std::vector<PointWithStamp>> {
904+
const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose);
905+
if (!obstacle_idx) return std::nullopt;
906+
const auto collision_traj_point = traj_points.at(obstacle_idx.value());
907+
const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start;
908+
909+
PointWithStamp collision_traj_point_with_stamp;
910+
collision_traj_point_with_stamp.stamp = object_time;
911+
collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x;
912+
collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y;
913+
collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z;
914+
std::vector<PointWithStamp> collision_points_vector{collision_traj_point_with_stamp};
915+
return collision_points_vector;
916+
}();
917+
918+
if (!collision_points) return std::nullopt;
919+
const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
920+
// check if obstacle is driving on the opposite direction
921+
if (tangent_vel < 0.0) return std::nullopt;
922+
return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,
923+
tangent_vel, normal_vel, collision_points.value()};
924+
}
925+
926+
std::optional<std::vector<CruiseObstacle>> ObstacleCruisePlannerNode::findYieldCruiseObstacles(
927+
const std::vector<Obstacle> & obstacles, const std::vector<TrajectoryPoint> & traj_points)
928+
{
929+
if (obstacles.empty() || traj_points.empty()) return std::nullopt;
930+
const auto & p = behavior_determination_param_;
931+
932+
std::vector<Obstacle> stopped_obstacles;
933+
std::vector<Obstacle> moving_obstacles;
934+
935+
std::for_each(
936+
obstacles.begin(), obstacles.end(),
937+
[&stopped_obstacles, &moving_obstacles, &p](const auto & o) {
938+
const bool is_moving =
939+
std::hypot(o.twist.linear.x, o.twist.linear.y) > p.stopped_obstacle_velocity_threshold;
940+
if (is_moving) {
941+
const bool is_within_lat_dist_threshold =
942+
o.lat_dist_from_obstacle_to_traj < p.yield_lat_distance_threshold;
943+
if (is_within_lat_dist_threshold) moving_obstacles.push_back(o);
944+
return;
945+
}
946+
// lat threshold is larger for stopped obstacles
947+
const bool is_within_lat_dist_threshold =
948+
o.lat_dist_from_obstacle_to_traj <
949+
p.yield_lat_distance_threshold + p.max_lat_dist_between_obstacles;
950+
if (is_within_lat_dist_threshold) stopped_obstacles.push_back(o);
951+
return;
952+
});
953+
954+
if (stopped_obstacles.empty() || moving_obstacles.empty()) return std::nullopt;
955+
956+
std::sort(
957+
stopped_obstacles.begin(), stopped_obstacles.end(), [](const auto & o1, const auto & o2) {
958+
return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance;
959+
});
960+
961+
std::sort(moving_obstacles.begin(), moving_obstacles.end(), [](const auto & o1, const auto & o2) {
962+
return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance;
963+
});
964+
965+
std::vector<CruiseObstacle> yield_obstacles;
966+
for (const auto & moving_obstacle : moving_obstacles) {
967+
for (const auto & stopped_obstacle : stopped_obstacles) {
968+
const bool is_moving_obs_behind_of_stopped_obs =
969+
moving_obstacle.ego_to_obstacle_distance < stopped_obstacle.ego_to_obstacle_distance;
970+
const bool is_moving_obs_ahead_of_ego_front =
971+
moving_obstacle.ego_to_obstacle_distance > vehicle_info_.vehicle_length_m;
972+
973+
if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue;
974+
975+
const double lateral_distance_between_obstacles = std::abs(
976+
moving_obstacle.lat_dist_from_obstacle_to_traj -
977+
stopped_obstacle.lat_dist_from_obstacle_to_traj);
978+
979+
const double longitudinal_distance_between_obstacles = std::abs(
980+
moving_obstacle.ego_to_obstacle_distance - stopped_obstacle.ego_to_obstacle_distance);
981+
982+
const double moving_obstacle_speed =
983+
std::hypot(moving_obstacle.twist.linear.x, moving_obstacle.twist.linear.y);
984+
985+
const bool are_obstacles_aligned =
986+
lateral_distance_between_obstacles < p.max_lat_dist_between_obstacles;
987+
const bool obstacles_collide_within_threshold_time =
988+
longitudinal_distance_between_obstacles / moving_obstacle_speed <
989+
p.max_obstacles_collision_time;
990+
if (are_obstacles_aligned && obstacles_collide_within_threshold_time) {
991+
const auto yield_obstacle = createYieldCruiseObstacle(moving_obstacle, traj_points);
992+
if (yield_obstacle) {
993+
yield_obstacles.push_back(*yield_obstacle);
994+
}
995+
}
996+
}
997+
}
998+
if (yield_obstacles.empty()) return std::nullopt;
999+
return yield_obstacles;
1000+
}
1001+
8321002
std::optional<std::vector<PointWithStamp>>
8331003
ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle(
8341004
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,

0 commit comments

Comments
 (0)