feat(obstacle_cruise_planner): prior generation of collision distance #5619

yuki-takagi-66 · 2023-11-17T08:08:48Z

Description

BEFORE:
The current implementation calculate the stop point by the closest collision point.
This cause inappropriate behavior for looped trajectories.

AFTER:

The followings are achieved by this PR,

Proper handling of collision in extended trajectory is achieved (It is the main motivation of this PR).
The stop point modification around the goal point is now working properly.
goal_extension_length and goal_extension_interval may be no longer used and safe_distance_margin and decimate_trajectory_step_length are used instead.

This PR similar to #5512, but this PR maintain more compatibility.
In other words, the function that included in the above PR This PR contains the similar feature to #4952, and therefore approaching on curve is now provided as default setting. is obsoleted.

Tests performed

psim tests and Tier4 internal tests was performed in success.
No degrade directly related to this PR founds.
https://evaluation.tier4.jp/evaluation/reports/52d22759-d1af-5896-b016-3b45e73d4138?project_id=prd_jt

Notes for reviewers

While no longer used parameters remain, I will remove these parameters after this PR has been agreed upon.

Interface changes

No interface changes.

Effects on system behavior

Minor bugs mentioned above have been fixed.
The slow down planning and the cruise planning may not changed.

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.

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

codecov · 2023-11-17T08:27:25Z

Codecov Report

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

Comparison is base (765a596) 15.32% compared to head (4ff7372) 15.35%.
Report is 121 commits behind head on main.

Files	Patch %	Lines
.../obstacle_cruise_planner/src/planner_interface.cpp	0.00%	22 Missing ⚠️
...ning/obstacle_cruise_planner/src/polygon_utils.cpp	0.00%	12 Missing ⚠️
planning/obstacle_cruise_planner/src/node.cpp	0.00%	6 Missing and 1 partial ⚠️
...rivable_area_expansion/drivable_area_expansion.cpp	40.00%	0 Missing and 3 partials ⚠️
planning/obstacle_cruise_planner/src/utils.cpp	40.00%	3 Missing ⚠️
...include/obstacle_cruise_planner/common_structs.hpp	0.00%	2 Missing ⚠️
...lude/obstacle_cruise_planner/planner_interface.hpp	0.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5619      +/-   ##
==========================================
+ Coverage   15.32%   15.35%   +0.02%     
==========================================
  Files        1721     1721              
  Lines      118559   118523      -36     
  Branches    37995    38078      +83     
==========================================
+ Hits        18169    18199      +30     
+ Misses      79657    79569      -88     
- Partials    20733    20755      +22

Flag	Coverage Δ		*Carryforward flag
differential	`8.39% <4.08%> (?)`
total	`15.35% <40.00%> (+0.03%)`	⬆️	Carriedforward from 069439b

*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 · 2023-11-23T14:31:39Z

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

+  std::optional<double> calcCollisionDistanceForStopObstacle(
+    const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
+    const Obstacle & obstacle) const;


Suggested change

std::optional<double> calcCollisionDistanceForStopObstacle(

const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,

const Obstacle & obstacle) const;

Seems this function declaration is not required.

takayuki5168 · 2023-11-23T14:39:41Z

planning/obstacle_cruise_planner/src/polygon_utils.cpp

+  const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
+  const Obstacle & obstacle)
+{
+  auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);


Suggested change

auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);

const auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);

takayuki5168 · 2023-11-23T14:39:58Z

planning/obstacle_cruise_planner/src/polygon_utils.cpp

+  for (size_t col_i = 0; col_i < traj_polys.size(); ++col_i) {
+    if (boost::geometry::intersects(traj_polys.at(col_i), obstacle_polygon)) {
+      try {
+        double prev_index_dist =


Suggested change

double prev_index_dist =

const double prev_index_dist =

takayuki5168 · 2023-11-23T14:41:13Z

planning/obstacle_cruise_planner/src/polygon_utils.cpp

+  auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);
+  for (size_t col_i = 0; col_i < traj_polys.size(); ++col_i) {
+    if (boost::geometry::intersects(traj_polys.at(col_i), obstacle_polygon)) {
+      try {


Why do you use try here?
I think we can handle the corner case without using try, which is a better implementation

takayuki5168 · 2023-11-23T14:44:12Z

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -401,12 +373,13 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(

  stop_planning_debug_info_.set(
    StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
-    closest_obstacle_dist - abs_ego_offset);  // TODO(murooka)
+    closest_stop_obstacle->dist_to_collide);  // TODO(murooka)


Is this correct? Seems dist_to_collide does not consider the following distances

the distance from the path start point and the ego position

the distance from the base link to the ego front position.

takayuki5168 · 2023-11-23T14:48:08Z

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

  : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
    shape(arg_shape),
-    collision_point(arg_collision_point)
+    collision_point(arg_collision_point),


Is it hard to remove the collision_point here so that the developer will not get confused about which (collision point or dist_to_collide) to use?
If this still exists here, it will be used for some calculation by mistake.

Actually, the nearest search is applied to this collision point, which is not expected.

autoware.universe/planning/obstacle_cruise_planner/src/planner_interface.cpp

Line 407 in cb409fa

motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point);

If this variable has to exist for the debug information, we'd better change the variable name to collision_point_for_debug for example so that the developer can understand the reason for this variable to exist.

takayuki5168 · 2023-11-23T14:55:20Z

planning/obstacle_cruise_planner/src/planner_interface.cpp

+  double dist_to_collide_on_ref_traj =
+    motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
+    closest_stop_obstacle->dist_to_collide;


This cannot consider the detailed ego position since ego_segment_idx means the resampled index for the ego position.

Also, the distance from the base link to the ego front position (abs_ego_offset) seems not considered with this PR's change.

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

…autowarefoundation#5619) * set goal extension param as unused * change to use collision point * fill out z coordinate value * handle with the backward paths Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

yuki-takagi-66 added 5 commits November 17, 2023 14:35

complete temp implementation

def8591

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

improve identity with previous codes

10c3224

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

remove dupulicate comments

504ba89

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

set goal extension param as unused

54e490d

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

remove debug macro, and aligne

cb409fa

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Nov 17, 2023

yuki-takagi-66 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Nov 17, 2023

yuki-takagi-66 changed the title ~~Feat/obstacle cruise/prior generation of collision distance~~ feat(obstacle cruise): prior generation of collision distance Nov 17, 2023

yuki-takagi-66 changed the title ~~feat(obstacle cruise): prior generation of collision distance~~ feat(obstacle_cruise_planner): prior generation of collision distance Nov 17, 2023

yuki-takagi-66 assigned takayuki5168 Nov 17, 2023

yuki-takagi-66 marked this pull request as ready for review November 17, 2023 08:24

yuki-takagi-66 requested review from kosuke55, satoshi-ota, takayuki5168 and purewater0901 as code owners November 17, 2023 08:24

yuki-takagi-66 mentioned this pull request Nov 20, 2023

feat(obstacle_cruise_planner)!: change logic to secure safe distance margin as polygons distance #5512

Closed

7 tasks

takayuki5168 reviewed Nov 23, 2023

View reviewed changes

yuki-takagi-66 added 5 commits November 24, 2023 21:25

change to use collision point

5e709c3

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

remove debug code

901ed6d

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

fill out z coordinate value

7549060

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

chore unused var

e9a7771

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

handle with the backward paths

069439b

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

takayuki5168 approved these changes Nov 30, 2023

View reviewed changes

yuki-takagi-66 added 2 commits November 30, 2023 19:00

fix typo and format

7ed9814

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

run pre-commit to alll

4ff7372

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

yuki-takagi-66 merged commit fd58f69 into autowarefoundation:main Dec 1, 2023

yuki-takagi-66 deleted the feat/obstacle_cruise/prior-generation-of-collision-distance branch December 1, 2023 04:29

This was referenced Dec 1, 2023

chore(obstacle_cruise)!: remove unused params autowarefoundation/autoware_launch#716

Merged

chore(obstacle_cruise)!: remove unused params #5738

Merged

tier4-autoware-public-bot bot mentioned this pull request Dec 6, 2023

chore: sync upstream tier4/autoware.universe#1059

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(obstacle_cruise_planner): prior generation of collision distance #5619

feat(obstacle_cruise_planner): prior generation of collision distance #5619

yuki-takagi-66 commented Nov 17, 2023 •

edited

Loading

codecov bot commented Nov 17, 2023 •

edited

Loading

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023 •

edited

Loading

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

takayuki5168 Nov 23, 2023

	std::optional<double> calcCollisionDistanceForStopObstacle(
	const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
	const Obstacle & obstacle) const;

	auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);
	const auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape);

feat(obstacle_cruise_planner): prior generation of collision distance #5619

feat(obstacle_cruise_planner): prior generation of collision distance #5619

Conversation

yuki-takagi-66 commented Nov 17, 2023 • 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 Nov 17, 2023 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

takayuki5168 Nov 23, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yuki-takagi-66 commented Nov 17, 2023 •

edited

Loading

codecov bot commented Nov 17, 2023 •

edited

Loading

takayuki5168 Nov 23, 2023 •

edited

Loading