feat(avoidance_by_lc): add new module to avoid obstacle by lane change

[satoshi-ota]

Description

⬇️ These PRs must be merged before this PR.
tier4/tier4_autoware_msgs#71
autowarefoundation/autoware_launch#261

Add new module avoidance_by_lc.
This module execute lane change maneuver when there are obstacles on ego's driving lane.

The obstacle filtering logic and path generation logic are almost the same to the avoidance and lane_change module.
(TODO: remove duplicated functions.)

Params

/**:
  ros__parameters:
    avoidance_by_lane_change:
      execute_object_num: 1               
      execute_object_longitudinal_margin: 0.0
      execute_only_when_lane_change_finish_before_object: true

• execute_object_num

This module execute lane change maneuver only when the target number is larger than execute_object_num. (If the condition is NOT satisfied, the nomal avoidance module will be running.)

• execute_object_longitudinal_margin

This module execute lane change maneuver only when the distance between the ego and object is larger than execute_object_longitudinal_margin. (If the condition is NOT satisfied, the nomal avoidance module will be running.)

• execute_only_when_lane_change_finish_before_object

If it set this param as true, this module will be canceled when the following conditions are satisfied.

1. The lane change end point is behind of the target object.
2. The path change is NOT approved.

execute_only_when_lane_change_finish_before_object: true

• Pros: since the lane change manauver is finished before the object, the path safety is almost guaranteed.
• Cons: depending the range of object recognition, it maybe NOT possible to execute lane change maneuver. The wider the detection range is, the longer the grace period to execute lane change maneuver is.

simplescreenrecorder-2023-03-21_15.34.23.mp4

---

execute_only_when_lane_change_finish_before_object: false

• Pros: can execute lane change at any timing.
• Cons: maybe output unsafe lane change path.

NOTE: This module also use avoidance and lane_change module params.

Related links

Tests performed

Please set following flag to FALSE, and confirm to pass all build.

autoware.universe/planning/behavior_path_planner/CMakeLists.txt

Line 10 in ad55953

set(COMPILE_WITH_OLD_ARCHITECTURE TRUE)

Use following PRs' branch.
tier4/tier4_autoware_msgs#71
autowarefoundation/autoware_launch#261

Set enable_module as true.

...
    avoidance_by_lc:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 3
      max_module_size: 1

simplescreenrecorder-2023-03-21_15.09.46.mp4

Notes for reviewers

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.

[VRichardJP]

I am not familiar with the way behavior modules interact with each other with the new design.

• Will this module have priority over the normal avoidance and lane change modules? In other words, are the normal avoidance/lane change modules skipped when this one can operate the lane change?
• With this module, would it make sense to disable normal obstacle avoidance if the road has alwways more than 2 driveable lanes?
• Does this module consider the required distance to change back to the initial lane? For example in this situation, there would not be enough distance left to come back to the initial lane so the normal avoidance should be used:
  

[satoshi-ota]

Hi @VRichardJP Thanks for your comments 👍
I'll answer your questions.

Q1. Will this module have priority over the normal avoidance and lane change modules? In other words, are the normal avoidance/lane change modules skipped when this one can operate the lane change?

Yes. I would like to set higher priority to this module than nomal avoidance and lane change module. So, as you said, these modules will be skipped in the following example scenario.

• lane changeable.
• want to execute lane change to right lane in order to arrive the goal.
• vehicle has to avoid object to right side.

-> This module will be executed, and the vehicle will move to right side lane. As a result, nomal avoidance and normal lane change module executions will be no longer needed.

Q2. With this module, would it make sense to disable normal obstacle avoidance if the road has alwways more than 2 driveable lanes?

It is not assumed that normal avoidance will not be used at all even if we enable this module in that scenario. I suppose there are some scenarios that are better handled by normal avoidance. For example,

• single vehicle avoidance -> normal avoidance
• multiple vehicle avoidance (such as following scene.) -> this module

Q3. Does this module consider the required distance to change back to the initial lane? For example in this situation, there would not be enough distance left to come back to the initial lane so the normal avoidance should be used:

This module checks the required distance to back to the original lane at least in following function, but the function doesn't consider the obsrtacle and It does not check to see if there is enough distance left to return to the original lane after completely avoiding the obstacle. I understand this issue should be resolved 👍

autoware.universe/planning/behavior_path_planner/src/util/lane_change/util.cpp

Lines 448 to 486 in 36bdca4

	bool hasEnoughDistance(
	const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
	[[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
	const Pose & goal_pose, const RouteHandler & route_handler,
	const double minimum_lane_change_length)
	{
	const double lane_change_total_distance = path.length.sum();
	const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back()));
	const auto overall_graphs = route_handler.getOverallGraphPtr();
	const double lane_change_required_distance =
	static_cast<double>(num) * minimum_lane_change_length;
	if (
	lane_change_total_distance + lane_change_required_distance >
	util::getDistanceToEndOfLane(current_pose, current_lanes)) {
	return false;
	}
	if (
	route_handler.isInGoalRouteSection(current_lanes.back()) &&
	lane_change_total_distance + lane_change_required_distance >
	util::getSignedDistance(current_pose, goal_pose, current_lanes)) {
	return false;
	}
	// return is there is no target lanes. Else continue checking
	if (target_lanes.empty()) {
	return true;
	}
	if (
	lane_change_total_distance + lane_change_required_distance >
	util::getDistanceToEndOfLane(current_pose, target_lanes)) {
	return false;
	}
	return true;
	}

[codecov]

Codecov Report

Patch coverage has no change and project coverage change: -0.09 ⚠️

Comparison is base (d2a348b) 12.43% compared to head (9c5022e) 12.34%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3125      +/-   ##
==========================================
- Coverage   12.43%   12.34%   -0.09%     
==========================================
  Files        1365     1368       +3     
  Lines       95296    95958     +662     
  Branches    27197    27529     +332     
==========================================
  Hits        11847    11847              
- Misses      70922    71583     +661     
- Partials    12527    12528       +1     

Flag	Coverage Δ		*Carryforward flag
differential	4.69% <0.00%> (?)
total	12.43% <ø> (+<0.01%)	⬆️	Carriedforward from d2a348b

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

Impacted Files	Coverage Δ
...havior_path_planner/behavior_path_planner_node.hpp	0.00% <ø> (ø)
...th_planner/scene_module/avoidance_by_lc/module.hpp	0.00% <0.00%> (ø)
..._path_planner/util/avoidance_by_lc/module_data.hpp	0.00% <0.00%> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.12% <0.00%> (-0.01%)	⬇️
...lanner/src/scene_module/avoidance_by_lc/module.cpp	0.00% <0.00%> (ø)
...ehavior_path_planner/src/util/lane_change/util.cpp	0.00% <0.00%> (ø)
planning/rtc_interface/src/rtc_interface.cpp	0.00% <0.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[zulfaqar-azmi-t4]

@satoshi-ota
I might be mistaken but the obstacle registration will be performed in both AvoidLC and Avoidance. Is there any consideration in the future to unify the obstacle related process?

[zulfaqar-azmi-t4]

Looks good

Single AvoidLC

cap-.2023-04-05-12-15-56.mp4

Double AvoidLC

cap-.2023-04-05-12-18-41.mp4

[satoshi-ota]

@satoshi-ota I might be mistaken but the obstacle registration will be performed in both AvoidLC and Avoidance. Is there any consideration in the future to unify the obstacle related process?

@zulfaqar-azmi-t4 Thanks for comment. You're right.
Unfortunately, there is no concrete future plan to unify these two module. But there are duplicate process to filter avoidance target in this module and normal avoidance module, and I think it is not so good. So, I wanna fix it if I have time 🥺

[zulfaqar-azmi-t4]

LGTM!

[rej55]

LGTM