feat(obstacle_cruise_planner): yield function for ocp #6242

danielsanchezaran · 2024-01-31T00:13:56Z

Description

This PR adds an (optional) yield functionality for the obstacle cruise planner. The yield action is performed by cruising behind vehicles on neighbor lanes that might cut in into the ego vehicle's current lane.

How it works:
The algorithm searches for cruise type obstacles that:
a) are moving faster than a speed threshold (parameter) and are in front of the ego.
b) have another cruise obstacle that is stopped in front of them.
c) will collide with the stopped obstacle within a given time (if they continue a straight path towards it)

Tests performed

Scenario Evaluation tests: TIER IV INTERNAL LINK

Several tests with PSim:

Example using time of collision (between static obstacle and moving obstacle) to decide to yield or not. default threshold time is 10 seconds.

yield1.mp4

Example 2

cap-.2024-01-25-15-38-52.mp4

Example 3

cap-.2024-01-25-16-31-33.mp4

Notes for reviewers

Requires changes to launch. autowarefoundation/autoware_launch#837

Interface changes

Effects on system behavior

If enabled, the ego will cruise behind neighbor cars that will likely cut in into the ego's lane.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

codecov · 2024-01-31T19:04:52Z

Codecov Report

Attention: 103 lines in your changes are missing coverage. Please review.

Comparison is base (0d10e81) 14.91% compared to head (8ee7e7a) 14.90%.
Report is 1 commits behind head on main.

Files	Patch %	Lines
planning/obstacle_cruise_planner/src/node.cpp	0.00%	94 Missing and 7 partials ⚠️
...include/obstacle_cruise_planner/common_structs.hpp	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6242      +/-   ##
==========================================
- Coverage   14.91%   14.90%   -0.02%     
==========================================
  Files        1817     1817              
  Lines      125357   125452      +95     
  Branches    37640    37697      +57     
==========================================
  Hits        18697    18697              
- Misses      85651    85739      +88     
- Partials    21009    21016       +7

Flag	Coverage Δ		*Carryforward flag
differential	`7.90% <0.00%> (?)`
total	`14.91% <ø> (+<0.01%)`	⬆️	Carriedforward from 0d10e81

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

takayuki5168 · 2024-02-02T09:32:56Z

planning/obstacle_cruise_planner/src/node.cpp

 bool isFrontObstacle(
  const std::vector<TrajectoryPoint> & traj_points, const size_t ego_idx,
-  const geometry_msgs::msg::Point & obstacle_pos)
+  const geometry_msgs::msg::Point & obstacle_pos, double * ego_to_obstacle_distance)
 {
  const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos);

-  const double ego_to_obstacle_distance =
-    motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx);
-
-  if (ego_to_obstacle_distance < 0) {
-    return false;
-  }
-
-  return true;
+  *ego_to_obstacle_distance = motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx);
+  return *ego_to_obstacle_distance >= 0.0;
 }


The reference argument is not recommended.
How about renaming the function to calcDistanceToFrontVehicle or something whose return value's type is std::optional<double> and return std::nullopt if the vehicle is not a front vehicle?

Done, thanks for the suggestion!

takayuki5168 · 2024-02-02T09:34:18Z

planning/obstacle_cruise_planner/src/node.cpp

@@ -491,9 +509,19 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
  //    (1) with a proper label
  //    (2) in front of ego
  //    (3) not too far from trajectory
-  const auto target_obstacles = convertToObstacles(traj_points);
+  const auto target_obstacles = [&]() {


better to rename to sorted_target_obstacles, so that other people will know this variable's order has an implicit meaning.

I decided to refactor a little and sort the moving and stopped obstacles in the findYieldCruiseObstacles function instead. I think it makes the code easier to read and it makes more sense semantically. Please let me know if you agree.

takayuki5168 · 2024-02-02T09:35:39Z

planning/obstacle_cruise_planner/src/node.cpp

+    double lat_dist_from_obstacle_to_traj;
    const double min_lat_dist_to_traj_poly = [&]() {
-      const double lat_dist_from_obstacle_to_traj =
+      lat_dist_from_obstacle_to_traj =
        motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);


const double lat_dist_from_obstacle_to_traj = motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position);

can be moved to the outside of the lambda function.

Done! Thanks for the review

takayuki5168 · 2024-02-05T02:50:18Z

planning/obstacle_cruise_planner/src/node.cpp

      RCLCPP_INFO_EXPRESSION(
        get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.",
        object_id.c_str());
      continue;
    }

    // 3. Check if rough lateral distance is smaller than the threshold
+    double lat_dist_from_obstacle_to_traj =


Suggested change

double lat_dist_from_obstacle_to_traj =

const double lat_dist_from_obstacle_to_traj =

Signed-off-by: Daniel Sanchez <[email protected]>

…tion#6242) * WIP make ego yield to cut in vehicles Signed-off-by: Daniel Sanchez <[email protected]> * param name change Signed-off-by: Daniel Sanchez <[email protected]> * add params Signed-off-by: Daniel Sanchez <[email protected]> * refactor Signed-off-by: Daniel Sanchez <[email protected]> * fix bug for distance comparison Signed-off-by: Daniel Sanchez <[email protected]> * implement a collision time check for yield candidates Signed-off-by: Daniel Sanchez <[email protected]> * deleting comments Signed-off-by: Daniel Sanchez <[email protected]> * comment out prints for test Signed-off-by: Daniel Sanchez <[email protected]> * delete comments, use param Signed-off-by: Daniel Sanchez <[email protected]> * Add param to set moving obstacle speed threshold Signed-off-by: Daniel Sanchez <[email protected]> * WIP check for cars going opposite direction Signed-off-by: Daniel Sanchez <[email protected]> * check opposite line driving Signed-off-by: Daniel Sanchez <[email protected]> * update documentation Signed-off-by: Daniel Sanchez <[email protected]> * update README Signed-off-by: Daniel Sanchez <[email protected]> * set default false Signed-off-by: Daniel Sanchez <[email protected]> * refactor Signed-off-by: Daniel Sanchez <[email protected]> * refactor isFrontObstacle Signed-off-by: Daniel Sanchez <[email protected]> * make const Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]> Signed-off-by: Daniel Sanchez <[email protected]>

feat(obstacle_cruise_planner): yield function for ocp (autowarefoundation#6242)

…tion#6242) * WIP make ego yield to cut in vehicles Signed-off-by: Daniel Sanchez <[email protected]> * param name change Signed-off-by: Daniel Sanchez <[email protected]> * add params Signed-off-by: Daniel Sanchez <[email protected]> * refactor Signed-off-by: Daniel Sanchez <[email protected]> * fix bug for distance comparison Signed-off-by: Daniel Sanchez <[email protected]> * implement a collision time check for yield candidates Signed-off-by: Daniel Sanchez <[email protected]> * deleting comments Signed-off-by: Daniel Sanchez <[email protected]> * comment out prints for test Signed-off-by: Daniel Sanchez <[email protected]> * delete comments, use param Signed-off-by: Daniel Sanchez <[email protected]> * Add param to set moving obstacle speed threshold Signed-off-by: Daniel Sanchez <[email protected]> * WIP check for cars going opposite direction Signed-off-by: Daniel Sanchez <[email protected]> * check opposite line driving Signed-off-by: Daniel Sanchez <[email protected]> * update documentation Signed-off-by: Daniel Sanchez <[email protected]> * update README Signed-off-by: Daniel Sanchez <[email protected]> * set default false Signed-off-by: Daniel Sanchez <[email protected]> * refactor Signed-off-by: Daniel Sanchez <[email protected]> * refactor isFrontObstacle Signed-off-by: Daniel Sanchez <[email protected]> * make const Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]> Signed-off-by: Daniel Sanchez <[email protected]>

github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:planning Route planning, decision-making, and navigation. (auto-assigned) labels Jan 31, 2024

danielsanchezaran mentioned this pull request Jan 31, 2024

feat(obstacle_cruise_planner): yield function for ocp autowarefoundation/autoware_launch#837

Merged

7 tasks

danielsanchezaran assigned danielsanchezaran and takayuki5168 Jan 31, 2024

danielsanchezaran marked this pull request as ready for review January 31, 2024 03:02

danielsanchezaran requested review from kosuke55, satoshi-ota, takayuki5168 and yuki-takagi-66 as code owners January 31, 2024 03:02

danielsanchezaran added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jan 31, 2024

takayuki5168 reviewed Feb 2, 2024

View reviewed changes

danielsanchezaran force-pushed the feat/yield-function-for-ocp branch from 5515d35 to 71755e1 Compare February 5, 2024 02:46

takayuki5168 reviewed Feb 5, 2024

View reviewed changes

takayuki5168 approved these changes Feb 5, 2024

View reviewed changes

danielsanchezaran and others added 12 commits February 5, 2024 15:24

WIP make ego yield to cut in vehicles

65361a7

Signed-off-by: Daniel Sanchez <[email protected]>

param name change

b319878

Signed-off-by: Daniel Sanchez <[email protected]>

add params

98ff292

Signed-off-by: Daniel Sanchez <[email protected]>

refactor

dc432b3

Signed-off-by: Daniel Sanchez <[email protected]>

fix bug for distance comparison

b0cc2de

Signed-off-by: Daniel Sanchez <[email protected]>

implement a collision time check for yield candidates

8e987a0

Signed-off-by: Daniel Sanchez <[email protected]>

deleting comments

6f7e664

Signed-off-by: Daniel Sanchez <[email protected]>

comment out prints for test

8432126

Signed-off-by: Daniel Sanchez <[email protected]>

delete comments, use param

46331e7

Signed-off-by: Daniel Sanchez <[email protected]>

Add param to set moving obstacle speed threshold

c64a18d

Signed-off-by: Daniel Sanchez <[email protected]>

WIP check for cars going opposite direction

380d44f

Signed-off-by: Daniel Sanchez <[email protected]>

check opposite line driving

bac1a17

Signed-off-by: Daniel Sanchez <[email protected]>

danielsanchezaran added 6 commits February 5, 2024 15:24

update documentation

13f84bd

Signed-off-by: Daniel Sanchez <[email protected]>

update README

65ce44b

Signed-off-by: Daniel Sanchez <[email protected]>

set default false

c33eb34

Signed-off-by: Daniel Sanchez <[email protected]>

refactor

9209372

Signed-off-by: Daniel Sanchez <[email protected]>

refactor isFrontObstacle

24c89af

Signed-off-by: Daniel Sanchez <[email protected]>

make const

8ee7e7a

Signed-off-by: Daniel Sanchez <[email protected]>

danielsanchezaran force-pushed the feat/yield-function-for-ocp branch from 47d64b6 to 8ee7e7a Compare February 5, 2024 06:24

danielsanchezaran merged commit 6b884ea into autowarefoundation:main Feb 5, 2024
19 of 24 checks passed

danielsanchezaran deleted the feat/yield-function-for-ocp branch February 5, 2024 08:08

tier4-autoware-public-bot bot mentioned this pull request Feb 7, 2024

chore: sync upstream tier4/autoware.universe#1130

Merged

danielsanchezaran mentioned this pull request Mar 4, 2024

feat(obstacle_cruise_planner): yield function for ocp (#6242) tier4/autoware.universe#1168

Merged

shmpwk added a commit to tier4/autoware.universe that referenced this pull request Mar 5, 2024

Merge pull request #1168 from tier4/cherry-pick/v0.44.3.0-yield-for-ocp

da58dc1

feat(obstacle_cruise_planner): yield function for ocp (autowarefoundation#6242)

danielsanchezaran mentioned this pull request Mar 12, 2024

feat(obstacle_cruise_planner): enable obstacle cruise's yield function by default autowarefoundation/autoware_launch#917

Merged

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(obstacle_cruise_planner): yield function for ocp #6242

feat(obstacle_cruise_planner): yield function for ocp #6242

danielsanchezaran commented Jan 31, 2024 •

edited

Loading

codecov bot commented Jan 31, 2024 •

edited

Loading

takayuki5168 Feb 2, 2024

danielsanchezaran Feb 5, 2024

takayuki5168 Feb 2, 2024

danielsanchezaran Feb 5, 2024

takayuki5168 Feb 2, 2024

danielsanchezaran Feb 5, 2024

takayuki5168 Feb 5, 2024

	double lat_dist_from_obstacle_to_traj =
	const double lat_dist_from_obstacle_to_traj =

feat(obstacle_cruise_planner): yield function for ocp #6242

feat(obstacle_cruise_planner): yield function for ocp #6242

Conversation

danielsanchezaran commented Jan 31, 2024 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented Jan 31, 2024 • edited Loading

Codecov Report

takayuki5168 Feb 2, 2024

Choose a reason for hiding this comment

danielsanchezaran Feb 5, 2024

Choose a reason for hiding this comment

takayuki5168 Feb 2, 2024

Choose a reason for hiding this comment

danielsanchezaran Feb 5, 2024

Choose a reason for hiding this comment

takayuki5168 Feb 2, 2024

Choose a reason for hiding this comment

danielsanchezaran Feb 5, 2024

Choose a reason for hiding this comment

takayuki5168 Feb 5, 2024

Choose a reason for hiding this comment

danielsanchezaran commented Jan 31, 2024 •

edited

Loading

codecov bot commented Jan 31, 2024 •

edited

Loading