diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
index bfe310b73725f..4ddd16a8f7d1e 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
@@ -7,11 +7,15 @@
detection_area_right_expand_dist: 0.0
detection_area_left_expand_dist: 1.0
+ enable_avoidance_over_same_direction: true
+ enable_avoidance_over_opposite_direction: true
+
threshold_distance_object_is_on_center: 1.0 # [m]
threshold_speed_object_is_stopped: 1.0 # [m/s]
object_check_forward_distance: 150.0 # [m]
object_check_backward_distance: 2.0 # [m]
- lateral_collision_margin: 2.0 # [m]
+ lateral_collision_margin: 1.0 # [m]
+ lateral_collision_safety_buffer: 0.7 # [m]
prepare_time: 2.0 # [s]
min_prepare_distance: 1.0 # [m]
@@ -20,6 +24,8 @@
min_nominal_avoidance_speed: 7.0 # [m/s]
min_sharp_avoidance_speed: 1.0 # [m/s]
+ road_shoulder_safety_margin: 0.5 # [m]
+
max_right_shift_length: 5.0
max_left_shift_length: 5.0
diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md
index 6a1fbe5f587c8..ed1adff4f6181 100644
--- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md
+++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md
@@ -238,6 +238,148 @@ The shift points are modified by a filtering process in order to get the expecte
- Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line.
- Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them.
+#### Computing shift length (available as of develop/v0.25.0)
+
+**Note**: This feature is available as of develop/`v0.25.0`.
+
+The shift length is set as a constant value before the feature is implemented (develop/`v0.24.0` and below). Setting the shift length like this will cause the module to generate an avoidance path regardless of actual environmental properties. For example, the path might exceed the actual road boundary or go towards a wall. Therefore, to address this limitation, in addition to [how to decide the target obstacle](#how-to-decide-the-target-obstacles), the upgraded module also takes into account the following additional element
+
+- The obstacles' current lane and position.
+- The road shoulder with reference to the direction to avoid.
+ - Note: Lane direction is disregarded.
+
+These elements are used to compute the distance from the object to the road's shoulder (`(class ObjectData.to_road_shoulder_distance)`).
+
+
+
+##### Computing shift length
+
+To compute the shift length, in addition to the vehicle's width and the parameterized `lateral_collision_margin`, the upgraded feature also adds two new parameters; `lateral_collision_safety_buffer` and `road_shoulder_safety_margin`.
+
+- The `lateral_collision_safety_buffer` parameter is used to set a safety gap that will act as the final line of defense when computing avoidance path.
+ - The rationale behind having this parameter is that the parameter `lateral_collision_margin` might be changed according to the situation for various reasons. Therefore, `lateral_collision_safety_buffer` will act as the final line of defense in case of the usage of `lateral_collision_margin` fails.
+ - It is recommended to set the value to more than half of the ego vehicle's width.
+- The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder.
+
+
+
+The shift length is subjected to the following constraints.
+
+
+
+$$
+\text{shift_length}=\begin{cases}d_{lcsb}+d_{lcm}+\frac{1}{2}(W_{ego})&\text{if}&(d_{lcsb}+d_{lcm}+W_{ego}+d_{rssm})\lt d_{trsd}\\0 &\textrm{if}&\left(d_{lcsb}+d_{lcm}+W_{ego}+d_{rssm}\right)\geq d_{trsd}\end{cases}
+$$
+
+where
+
+- 
= `lateral_collision_safety_buffer`
+- 
= `lateral_collision_margin`
+- 
= ego vehicle's width
+- 
= `road_shoulder_safety_margin`
+- 
= `(class ObjectData).to_road_shoulder_distance`
+
+```plantuml
+@startuml
+skinparam monochrome true
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title avoidance path planning
+start
+partition calcAvoidanceTargetObjects() {
+:calcOverhangDistance;
+note right
+ - get overhang_pose
+end note
+
+:getClosestLaneletWithinRoute;
+note right
+ - obtain overhang_lanelet
+end note
+
+if(Is overhang_lanelet.id() exist?) then (no)
+stop
+else (\n yes)
+
+if(isOnRight(obstacle)?) then (yes)
+partition getLeftMostLineString() {
+repeat
+repeat
+:getLeftLanelet;
+note left
+ Check both
+ - lane-changeable lane
+ - non lane-changeable lane
+end note
+repeat while (Same direction Lanelet exist?) is (yes) not (no)
+:getLeftOppositeLanelet;
+repeat while (Opposite direction Lanelet exist?) is (yes) not (no)
+}
+:compute\n(class ObjectData).to_road_shoulder_distance;
+note left
+distance from overhang_pose
+to left most linestring
+end note
+else(\n no)
+partition getrightMostLineString() {
+repeat
+repeat
+:getRightLanelet;
+note right
+ Check both
+ - lane-changeable lane
+ - non lane-changeable lane
+end note
+repeat while (Same direction Lanelet exist?) is (yes) not (no)
+:getRightOppositeLanelet;
+repeat while (Opposite direction Lanelet exist?) is (yes) not (no)
+}
+:compute\n(class ObjectData).to_road_shoulder_distance;
+note right
+distance from overhang_pose
+to right most linestring
+end note
+endif
+endif
+}
+
+partition calcRawShiftPointsFromObjects() {
+:compute max_allowable_lateral_distance;
+note right
+The sum of
+- lat_collision_safety_buffer
+- lat_collision_margin
+- vehicle_width
+end note
+:compute max_shift_length;
+note right
+subtract
+- road_shoulder_safety_margin
+- 0.5 x vehicle_width
+from (class ObjectData).to_road_shoulder_margin
+end note
+if(isOnRight(object)?) then (yes)
+if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes)
+:max_left_shift_limit = max_shift_length;
+else (\n No)
+:max_left_shift_limit = 0.0;
+endif
+else (\n No)
+if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes)
+:max_right_shift_limit = -max_shift_length;
+else (\n No)
+:max_right_shift_limit = 0.0;
+endif
+endif
+:compute shift length;
+}
+stop
+@enduml
+```
+
+
+
#### How to keep the consistency of the planning
TODO
@@ -250,7 +392,8 @@ TODO
| :----------------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 |
| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 3.0 |
-| lateral_collision_margin | [m] | double | The lateral distance between ego and avoidance targets. | 2.0 |
+| lateral_collision_margin | [m] | double | The lateral distance between ego and avoidance targets. | 1.5 |
+| lateral_collision_safety_buffer | [m] | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle | 0.5 |
| longitudinal_collision_margin_min_distance | [m] | double | when complete avoidance motion, there is a distance margin with the object for longitudinal direction. | 0.0 |
| longitudinal_collision_margin_time | [s] | double | when complete avoidance motion, there is a time margin with the object for longitudinal direction. | 0.0 |
| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 1.0 |
@@ -262,6 +405,7 @@ TODO
| min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 |
| max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 |
| max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 |
+| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.5 |
### Speed limit modification
diff --git a/planning/behavior_path_planner/behavior_path_planner_design_limitations.md b/planning/behavior_path_planner/behavior_path_planner_design_limitations.md
new file mode 100644
index 0000000000000..7ffd89f2b495c
--- /dev/null
+++ b/planning/behavior_path_planner/behavior_path_planner_design_limitations.md
@@ -0,0 +1,52 @@
+# Design limitation
+
+The document describes the limitations that are currently present in the `behavior_path_planner` module.
+
+The following items (but not limited to) fall in the scope of limitation:
+
+- limitations due to the third-party API design and requirement
+- limitations due to any shortcoming out of the developer's control.
+
+## Limitation: Multiple connected opposite lanes require Linestring with shared ID
+
+To fully utilize the `Lanelet2`'s [API](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md#lanelet), the design of the vector map (`.osm`) needs to follow all the criteria described in `Lanelet2` documentation. Specifically, in the case of 2 or more lanes, the Linestrings that divide the current lane with the opposite/adjacent lane need to have a matching `Linestring ID`. Assume the following **ideal case**.
+
+
+
+In the image, `Linestring ID51` is shared by `Lanelet A` and `Lanelet B`. Hence we can directly use the available `left`, `adjacentLeft`, `right`, `adjacentRight` and `findUsages` method within `Lanelet2`'s API to directly query the direction and opposite lane availability.
+
+```cpp
+const auto right_lane = routing_graph_ptr_->right(lanelet);
+const auto adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet);
+const auto opposite_right_lane = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert());
+```
+
+The following images show the situation where **these API does not work directly**. This means that we cannot use them straight away, and several assumptions and logical instruction are needed to make these APIs work.
+
+
+
+In this example (multiple linestring issues), `Lanelet C` contains `Linestring ID61` and `ID62`, while `Lanelet D` contains `Linestring ID63` and `ID 64`. Although the `Linestring ID62` and `ID64` have identical point IDs and seem visually connected, the API will treat these Linestring as though they are separated. When it searches for any `Lanelet` that is connected via `Linestring ID62`, it will return `NULL`, since `ID62` only connects to `Lanelet C` and not other `Lanelet`.
+
+Although, in this case, it is possible to forcefully search the lanelet availability by checking the lanelet that contains the points, using`getLaneletFromPoint` method. But, the implementation requires complex rules for it to work. Take the following images as an example.
+
+
+
+Assume `Object X` is in `Lanelet F`. We can forcefully search `Lanelet E` via `Point 7`, and it will work if `Point 7` is utilized by **only 2 lanelet**. However, the complexity increases when we want to start searching for the **direction** of the opposite lane. We can infer the direction of the lanelet by using mathematical operations (dot product of vector `V_ID72` (`Point 6` minus `Point 9`), and `V_ID74` (`Point 7` minus `Point 8`). But, notice that we did not use Point 7 in V_ID72. This is because searching it requires an iteration, adding additional non-beneficial computation.
+
+Suppose the points are used by **more than 2 lanelets**. In that case, we have to find the differences for all lanelet, and the result might be undefined. The reason is that the differences between the coordinates do not reflect the actual **shape** of the lanelet. The following image demonstrates this point.
+
+
+
+
+
+There are many other available solutions to try. However, further attempt to solve this might cause issues in the future, especially for maintaining or scaling up the software.
+
+In conclusion, the multiple Linestring issues will not be supported. Covering these scenarios might give the user an "everything is possible" impression. This is dangerous since any attempt to create a non-standardized vector map is not compliant with safety regulations.
+
+## Limitation: Avoidance at Corners and Intersections
+
+Currently, the implementation doesn't cover avoidance at corners and intersections. The reason is similar to here. However, this case can still be supported in the future (assuming the vector map is defined correctly).
+
+
+
+
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
index bfe310b73725f..34951cc91d30a 100644
--- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
+++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -7,11 +7,15 @@
detection_area_right_expand_dist: 0.0
detection_area_left_expand_dist: 1.0
+ enable_avoidance_over_same_direction: true
+ enable_avoidance_over_opposite_direction: true
+
threshold_distance_object_is_on_center: 1.0 # [m]
threshold_speed_object_is_stopped: 1.0 # [m/s]
object_check_forward_distance: 150.0 # [m]
object_check_backward_distance: 2.0 # [m]
- lateral_collision_margin: 2.0 # [m]
+ lateral_collision_margin: 1.0 # [m]
+ lateral_collision_safety_buffer: 0.7 # [m]
prepare_time: 2.0 # [s]
min_prepare_distance: 1.0 # [m]
@@ -20,6 +24,8 @@
min_nominal_avoidance_speed: 7.0 # [m/s]
min_sharp_avoidance_speed: 1.0 # [m/s]
+ road_shoulder_safety_margin: 0.5 # [m]
+
max_right_shift_length: 5.0
max_left_shift_length: 5.0
@@ -30,7 +36,7 @@
# For prevention of large acceleration while avoidance
min_avoidance_speed_for_acc_prevention: 3.0 # [m/s]
- max_avoidance_acceleration: 0.5 # [m/ss]
+ max_avoidance_acceleration: 0.5 # [m/s2]
# for debug
publish_debug_marker: false
diff --git a/planning/behavior_path_planner/image/limitations/limitation-corner.png b/planning/behavior_path_planner/image/limitations/limitation-corner.png
new file mode 100644
index 0000000000000..fe32071edd6dc
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation-corner.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation-intersection.png b/planning/behavior_path_planner/image/limitations/limitation-intersection.png
new file mode 100644
index 0000000000000..eb78094b51655
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation-intersection.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation01-01.png b/planning/behavior_path_planner/image/limitations/limitation01-01.png
new file mode 100644
index 0000000000000..a84ec07ce940b
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-01.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png b/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png
new file mode 100644
index 0000000000000..c728f188bb387
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png b/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png
new file mode 100644
index 0000000000000..8d594e0ca5d94
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png b/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png
new file mode 100644
index 0000000000000..79daca2b4f502
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png differ
diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png b/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png
new file mode 100644
index 0000000000000..cd734fcd1e310
Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png differ
diff --git a/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png b/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png
new file mode 100644
index 0000000000000..724708d23ff80
Binary files /dev/null and b/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png differ
diff --git a/planning/behavior_path_planner/image/shift_length_computation.png b/planning/behavior_path_planner/image/shift_length_computation.png
new file mode 100644
index 0000000000000..5c6f364ececd0
Binary files /dev/null and b/planning/behavior_path_planner/image/shift_length_computation.png differ
diff --git a/planning/behavior_path_planner/image/shift_length_parameters.png b/planning/behavior_path_planner/image/shift_length_parameters.png
new file mode 100644
index 0000000000000..4bb16c03b0881
Binary files /dev/null and b/planning/behavior_path_planner/image/shift_length_parameters.png differ
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index ea33116ea3965..3e3db6f912043 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -135,7 +135,7 @@ class AvoidanceModule : public SceneModuleInterface
// -- path generation --
ShiftedPath generateAvoidancePath(PathShifter & shifter) const;
- void generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const;
+ void generateExtendedDrivableArea(ShiftedPath * shifted_path) const;
// -- velocity planning --
void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path) const;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
index 5c9ea12dbc429..3dee936f3b6a1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -52,6 +52,13 @@ struct AvoidanceParameters
// lanelet expand length for left side to find avoidance target vehicles
double detection_area_left_expand_dist = 1.0;
+ // enable avoidance to be perform only in lane with same direction
+ bool enable_avoidance_over_same_direction{true};
+
+ // enable avoidance to be perform in opposite lane direction
+ // to use this, enable_avoidance_over_same_direction need to be set to true.
+ bool enable_avoidance_over_opposite_direction{true};
+
// Vehicles whose distance to the center of the path is
// less than this will not be considered for avoidance.
double threshold_distance_object_is_on_center;
@@ -67,6 +74,9 @@ struct AvoidanceParameters
// we want to keep this lateral margin when avoiding
double lateral_collision_margin;
+ // a buffer in case lateral_collision_margin is set to 0. Will throw error
+ // don't ever set this value to 0
+ double lateral_collision_safety_buffer{0.5};
// when complete avoidance motion, there is a distance margin with the object
// for longitudinal direction
@@ -96,6 +106,10 @@ struct AvoidanceParameters
// distance for avoidance. Need a sharp avoidance path to avoid the object.
double min_sharp_avoidance_speed;
+ // The margin is configured so that the generated avoidance trajectory does not come near to the
+ // road shoulder.
+ double road_shoulder_safety_margin{1.0};
+
// Even if the obstacle is very large, it will not avoid more than this length for right direction
double max_right_shift_length;
@@ -152,6 +166,15 @@ struct ObjectData // avoidance target
// count up when object disappeared. Removed when it exceeds threshold.
int lost_count = 0;
+
+ // store the information of the lanelet which the object's overhang is currently occupying
+ lanelet::ConstLanelet overhang_lanelet;
+
+ // the position of the overhang
+ Pose overhang_pose;
+
+ // lateral distance from overhang to the road shoulder
+ double to_road_shoulder_distance{0.0};
};
using ObjectDataArray = std::vector;
@@ -242,6 +265,7 @@ struct DebugData
{
std::shared_ptr expanded_lanelets;
std::shared_ptr current_lanelets;
+ std::shared_ptr farthest_linestring_from_overhang;
AvoidPointArray current_shift_points; // in path shifter
AvoidPointArray new_shift_points; // in path shifter
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp
index 683677f3086ca..1181006d6fd61 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp
@@ -46,7 +46,8 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa
double calcDistanceToClosestFootprintPoint(
const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos);
-double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose);
+double calcOverhangDistance(
+ const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
void setEndData(
AvoidPoint & ap, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
index c732e810e4104..26abdd6978620 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
@@ -90,6 +90,12 @@ MarkerArray createPoseMarkerArray(
const Pose & pose, const std::string & ns, const int64_t id, const double r, const double g,
const double b);
+MarkerArray makeOverhangToRoadShoulderMarkerArray(
+ const behavior_path_planner::ObjectDataArray & objects);
+
+MarkerArray createOvehangFurthestLineStringMarkerArray(
+ const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
+ const double g, const double b);
} // namespace marker_utils
std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index 2dd50d50a410d..fcafa0144dd66 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -204,12 +204,15 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
p.resample_interval_for_output = dp("resample_interval_for_output", 3.0);
p.detection_area_right_expand_dist = dp("detection_area_right_expand_dist", 0.0);
p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0);
+ p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true);
+ p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true);
p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0);
p.object_check_forward_distance = dp("object_check_forward_distance", 150.0);
p.object_check_backward_distance = dp("object_check_backward_distance", 2.0);
p.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
+ p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);
p.prepare_time = dp("prepare_time", 3.0);
p.min_prepare_distance = dp("min_prepare_distance", 10.0);
@@ -218,6 +221,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
p.min_nominal_avoidance_speed = dp("min_nominal_avoidance_speed", 5.0);
p.min_sharp_avoidance_speed = dp("min_sharp_avoidance_speed", 1.0);
+ p.road_shoulder_safety_margin = dp("road_shoulder_safety_margin", 0.5);
+
p.max_right_shift_length = dp("max_right_shift_length", 1.5);
p.max_left_shift_length = dp("max_left_shift_length", 1.5);
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 7f7f5bb9120a2..5649c7e2a4025 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -156,6 +156,10 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
const lanelet::ConstLanelets & current_lanes, const PathWithLaneId & reference_path,
DebugData & debug) const
{
+ using lanelet::geometry::distance2d;
+ using lanelet::utils::getId;
+ using lanelet::utils::to2D;
+
const auto & path_points = reference_path.points;
const auto & ego_pos = getEgoPosition();
@@ -183,6 +187,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position)
: std::numeric_limits::max();
+ lanelet::ConstLineStrings3d debug_linestring;
+ debug_linestring.clear();
// for filtered objects
ObjectDataArray target_objects;
for (const auto & i : lane_filtered_objects_index) {
@@ -221,18 +227,66 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
const auto object_closest_pose = path_points.at(object_closest_index).point.pose;
object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos);
- // Object is on center line -> ignore.
- if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) {
- DEBUG_PRINT("Ignore object: (object is on center line)");
+ // Find the footprint point closest to the path, set to object_data.overhang_distance.
+ object_data.overhang_dist =
+ calcOverhangDistance(object_data, object_closest_pose, object_data.overhang_pose.position);
+
+ lanelet::ConstLanelet overhang_lanelet;
+ if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) {
continue;
}
- // Find the footprint point closest to the path, set to object_data.overhang_distance.
- object_data.overhang_dist = calcOverhangDistance(object_data, object_closest_pose);
+ if (overhang_lanelet.id()) {
+ object_data.overhang_lanelet = overhang_lanelet;
+ lanelet::BasicPoint3d overhang_basic_pose(
+ object_data.overhang_pose.position.x, object_data.overhang_pose.position.y,
+ object_data.overhang_pose.position.z);
+ if (isOnRight(object_data)) {
+ const auto & target_left_line = [this, &rh, &overhang_lanelet]() {
+ if (
+ parameters_.enable_avoidance_over_same_direction &&
+ parameters_.enable_avoidance_over_opposite_direction) {
+ return rh->getLeftMostLinestring(overhang_lanelet);
+ } else if (
+ parameters_.enable_avoidance_over_same_direction &&
+ !parameters_.enable_avoidance_over_opposite_direction) {
+ return rh->getLeftMostSameDirectionLinestring(overhang_lanelet);
+ }
+ return overhang_lanelet.leftBound();
+ }();
+ object_data.to_road_shoulder_distance =
+ distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString()));
+ debug_linestring.push_back(target_left_line);
+ } else {
+ const auto & target_right_line = [this, &rh, &overhang_lanelet]() {
+ if (
+ parameters_.enable_avoidance_over_same_direction &&
+ parameters_.enable_avoidance_over_opposite_direction) {
+ return rh->getRightMostLinestring(overhang_lanelet);
+ } else if (
+ parameters_.enable_avoidance_over_same_direction &&
+ !parameters_.enable_avoidance_over_opposite_direction) {
+ return rh->getRightMostSameDirectionLinestring(overhang_lanelet);
+ }
+ return overhang_lanelet.rightBound();
+ }();
+ object_data.to_road_shoulder_distance =
+ distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString()));
+ debug_linestring.push_back(target_right_line);
+ }
+ }
DEBUG_PRINT(
- "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f",
- object_data.longitudinal, object_data.lateral, object_data.overhang_dist);
+ "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f,"
+ "to_road_shoulder_distance = %f",
+ object_data.longitudinal, object_data.lateral, object_data.overhang_dist,
+ object_data.to_road_shoulder_distance);
+
+ // Object is on center line -> ignore.
+ if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) {
+ DEBUG_PRINT("Ignore object: (object is on center line)");
+ continue;
+ }
// set data
target_objects.push_back(object_data);
@@ -240,6 +294,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
// debug
{
+ debug.farthest_linestring_from_overhang =
+ std::make_shared(debug_linestring);
debug.current_lanelets = std::make_shared(current_lanes);
debug.expanded_lanelets = std::make_shared(expanded_lanelets);
}
@@ -393,20 +449,42 @@ void AvoidanceModule::registerRawShiftPoints(const AvoidPointArray & future)
AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
const ObjectDataArray & objects) const
{
- const auto avoid_margin =
- parameters_.lateral_collision_margin + 0.5 * planner_data_->parameters.vehicle_width;
const auto prepare_distance = getNominalPrepareDistance();
// To be consistent with changes in the ego position, the current shift length is considered.
const auto current_ego_shift = getCurrentShift();
+ // // implement lane detection here.
+ const auto & lat_collision_safety_buffer = parameters_.lateral_collision_safety_buffer;
+ const auto & lat_collision_margin = parameters_.lateral_collision_margin;
+ const auto & vehicle_width = planner_data_->parameters.vehicle_width;
+ const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin;
+ const auto max_allowable_lateral_distance =
+ lat_collision_safety_buffer + lat_collision_margin + vehicle_width;
+
+ const auto avoid_margin =
+ lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;
AvoidPointArray avoid_points;
for (auto & o : objects) {
- // calc shift length with margin and shift limit
- const auto shift_length = isOnRight(o)
- ? std::min(o.overhang_dist + avoid_margin, getLeftShiftBound())
- : std::max(o.overhang_dist - avoid_margin, getRightShiftBound());
+ const auto max_shift_length =
+ o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width;
+ const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
+ this]() noexcept {
+ const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length);
+ return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint
+ : 0.0;
+ };
+
+ const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
+ this]() noexcept {
+ const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length);
+ return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint
+ : 0.0;
+ };
+ const auto shift_length = isOnRight(o)
+ ? std::min(o.overhang_dist + avoid_margin, max_left_shift_limit())
+ : std::max(o.overhang_dist - avoid_margin, max_right_shift_limit());
const auto avoiding_shift = shift_length - current_ego_shift;
const auto return_shift = shift_length;
@@ -1584,22 +1662,211 @@ double AvoidanceModule::getLeftShiftBound() const
return parameters_.max_left_shift_length;
}
-void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const
+// TODO(murooka) judge when and which way to extend drivable area. current implementation is keep
+// extending during avoidance module
+// TODO(murooka) freespace during turning in intersection where there is no neighbour lanes
+// NOTE: Assume that there is no situation where there is an object in the middle lane of more than
+// two lanes since which way to avoid is not obvious
+void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const
{
- const auto right_extend_elem =
- std::min_element(shifted_path->shift_length.begin(), shifted_path->shift_length.end());
- const auto left_extend_elem =
- std::max_element(shifted_path->shift_length.begin(), shifted_path->shift_length.end());
+ const auto & route_handler = planner_data_->route_handler;
+ lanelet::ConstLanelets extended_lanelets = avoidance_data_.current_lanelets;
+
+ {
+ // 0. Extend to right/left of objects
+ for (const auto & obstacle : avoidance_data_.objects) {
+ auto object_lanelet = obstacle.overhang_lanelet;
+ if (isOnRight(obstacle)) {
+ auto lanelet_at_left = route_handler->getLeftLanelet(object_lanelet);
+ while (lanelet_at_left) {
+ extended_lanelets.push_back(lanelet_at_left.get());
+ lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+ }
+ auto lanelet_at_right =
+ planner_data_->route_handler->getRightLanelet(lanelet_at_left.get());
+ while (lanelet_at_right) {
+ extended_lanelets.push_back(lanelet_at_right.get());
+ lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+ }
+ } else {
+ auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet);
+ while (lanelet_at_right) {
+ extended_lanelets.push_back(lanelet_at_right.get());
+ lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+ }
+ auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get());
+ while (lanelet_at_left) {
+ extended_lanelets.push_back(lanelet_at_left.get());
+ lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+ }
+ }
+ }
+ }
+
+ for (const auto & lane : avoidance_data_.current_lanelets) {
+ { // 1. extend to right/left or adjacent right/left (where lane_change tag = no, but not a
+ // problem to extend for avoidance) lane if it exists
+ // this can be available only if line string is shared
+ const auto opt_right_lane = route_handler->getRightLanelet(lane);
+ const auto opt_left_lane = route_handler->getLeftLanelet(lane);
+
+ if (opt_right_lane) {
+ extended_lanelets.push_back(opt_right_lane.get());
+ continue;
+ } else if (opt_left_lane) {
+ extended_lanelets.push_back(opt_left_lane.get());
+ continue;
+ }
+ }
+
+ { // 2. when there are multiple turning lanes whose previous lanelet is the same in
+ // intersection
+ const bool update_extended_lanelets = [&]() {
+ // lanelet is not turning lane
+ const std::string turn_direction = lane.attributeOr("turn_direction", "none");
+ if (turn_direction != "right" && turn_direction != "left") {
+ return false;
+ }
+
+ // get previous lane, and return false if previous lane does not exist
+ lanelet::ConstLanelet prev_lane;
+ if (!route_handler->getPreviousLaneletWithinRoute(lane, &prev_lane)) {
+ return false;
+ }
- double right_extend = std::min(*right_extend_elem, 0.0);
- double left_extend = std::max(*left_extend_elem, 0.0);
+ // get next lanes from the previous lane, and return false if next lanes do not exist
+ const auto next_lanes = route_handler->getNextLanelets(lane);
+ if (next_lanes.empty()) {
+ return false;
+ }
+
+ // look for neighbour lane, where end line of the lane is connected to end line of the
+ // original lane
+ for (const auto & next_lane : next_lanes) {
+ if (lane.id() == next_lane.id()) {
+ continue;
+ }
+
+ const Eigen::Vector2d & next_left_back_point_2d =
+ next_lane.leftBound2d().back().basicPoint();
+ const Eigen::Vector2d & next_right_back_point_2d =
+ next_lane.rightBound2d().back().basicPoint();
+
+ const Eigen::Vector2d & orig_left_back_point_2d = lane.leftBound2d().back().basicPoint();
+ const Eigen::Vector2d & orig_right_back_point_2d =
+ lane.rightBound2d().back().basicPoint();
+
+ constexpr double epsilon = 1e-5;
+ const bool is_neighbour_lane =
+ (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon ||
+ (next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon;
+ if (is_neighbour_lane) {
+ extended_lanelets.push_back(next_lane);
+ return true;
+ }
+ }
+
+ return false;
+ }();
+ if (update_extended_lanelets) {
+ continue;
+ }
+ }
- constexpr double THRESHOLD = 0.01;
- right_extend -= (right_extend < -THRESHOLD) ? margin : 0.0;
- left_extend += (left_extend > THRESHOLD) ? margin : 0.0;
+ { // 3. deal with the problem that line string is not shared to neighbour lanelets in
+ // intersection (for left lane), assuming that points are shared
+ // this part will be removed when the map format is modified correctly wrt sharing line string
+ // since 1 works for this
+ bool update_extended_lanelets = false;
+ const auto & left_lane_candidates =
+ route_handler->getLaneletsFromPoint(lane.leftBound().front());
+ for (const auto & left_lane_candidate : left_lane_candidates) {
+ const Eigen::Vector2d & left_lane_right_back_point_2d =
+ left_lane_candidate.rightBound2d().back().basicPoint();
+ const Eigen::Vector2d & orig_lane_left_back_point_2d =
+ lane.leftBound2d().back().basicPoint();
+
+ const double epsilon = 1e-5;
+ const bool is_neighbour_lane =
+ (left_lane_right_back_point_2d - orig_lane_left_back_point_2d).norm() < epsilon;
+ if (is_neighbour_lane) {
+ extended_lanelets.push_back(left_lane_candidate);
+ update_extended_lanelets = true;
+ break;
+ }
+ }
+ if (update_extended_lanelets) {
+ continue;
+ }
+ }
- const auto extended_lanelets = lanelet::utils::getExpandedLanelets(
- avoidance_data_.current_lanelets, left_extend, right_extend);
+ { // 4. deal with the problem that line string is not shared to neighbour lanelets in
+ // intersection (for right lane), assuming that points are shared
+ // this part will be removed if the map format is modified correctly wrt sharing line string
+ // since 1 works for this
+ bool update_extended_lanelets = false;
+ const auto & right_lane_candidates =
+ route_handler->getLaneletsFromPoint(lane.rightBound().front());
+ for (const auto & right_lane_candidate : right_lane_candidates) {
+ const Eigen::Vector2d & right_lane_left_back_point_2d =
+ right_lane_candidate.leftBound2d().back().basicPoint();
+ const Eigen::Vector2d & orig_lane_right_back_point_2d =
+ lane.rightBound2d().back().basicPoint();
+
+ const double epsilon = 1e-5;
+ const bool is_neighbour_lane =
+ (right_lane_left_back_point_2d - orig_lane_right_back_point_2d).norm() < epsilon;
+ if (is_neighbour_lane) {
+ extended_lanelets.push_back(right_lane_candidate);
+ update_extended_lanelets = true;
+ break;
+ }
+ }
+ if (update_extended_lanelets) {
+ continue;
+ }
+ }
+
+ {
+ // 5. if drivable area cannot be extended inside the same-direction lane, extend to even
+ // opposite lane
+ const auto opposite_lanes = route_handler->getRightOppositeLanelets(lane);
+
+ if (!opposite_lanes.empty()) {
+ for (const auto & opposite_lane : opposite_lanes) {
+ extended_lanelets.push_back(opposite_lane);
+ }
+ continue;
+ }
+ }
+
+ { // 6. deal with the problem that line string is not shared to neighbour opposite lanelet,
+ // assuming that points are shared
+ // this part will be removed when the map format is modified correctly wrt sharing line string
+ // since 5 works for this
+ bool update_extended_lanelets = false;
+ const auto & opposite_lane_candidates =
+ route_handler->getLaneletsFromPoint(lane.rightBound().front());
+ for (const auto & opposite_lane_candidate : opposite_lane_candidates) {
+ const Eigen::Vector2d & opposite_lane_right_front_point_2d =
+ opposite_lane_candidate.rightBound2d().front().basicPoint();
+ const Eigen::Vector2d & orig_lane_right_back_point_2d =
+ lane.rightBound2d().back().basicPoint();
+
+ const double epsilon = 1e-5;
+ const bool is_neighbour_lane =
+ (opposite_lane_right_front_point_2d - orig_lane_right_back_point_2d).norm() < epsilon;
+ if (is_neighbour_lane) {
+ extended_lanelets.push_back(opposite_lane_candidate);
+ update_extended_lanelets = true;
+ break;
+ }
+ }
+ if (update_extended_lanelets) {
+ continue;
+ }
+ }
+ }
{
const auto & p = planner_data_->parameters;
@@ -1821,8 +2088,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
debug_data_.output_shift = avoidance_path.shift_length;
// Drivable area generation.
- constexpr double extend_margin = 0.5;
- generateExtendedDrivableArea(&avoidance_path, extend_margin);
+ generateExtendedDrivableArea(&avoidance_path);
// modify max speed to prevent acceleration in avoidance maneuver.
modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path);
@@ -2343,10 +2609,12 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
using marker_utils::createAvoidPointMarkerArray;
using marker_utils::createLaneletsAreaMarkerArray;
using marker_utils::createObjectsMarkerArray;
+ using marker_utils::createOvehangFurthestLineStringMarkerArray;
using marker_utils::createPathMarkerArray;
using marker_utils::createPoseMarkerArray;
using marker_utils::createShiftLengthMarkerArray;
using marker_utils::createShiftPointMarkerArray;
+ using marker_utils::makeOverhangToRoadShoulderMarkerArray;
debug_marker_.markers.clear();
@@ -2373,6 +2641,9 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0));
add(createAvoidanceObjectsMarkerArray(avoidance_data_.objects, "avoidance_object"));
+ add(makeOverhangToRoadShoulderMarkerArray(avoidance_data_.objects));
+ add(createOvehangFurthestLineStringMarkerArray(
+ *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0));
// parent object info
addAvoidPoint(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0);
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp
index 623f0c97200b1..e9865b069eeca 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp
@@ -141,7 +141,8 @@ double calcDistanceToClosestFootprintPoint(
return distance;
}
-double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose)
+double calcOverhangDistance(
+ const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose)
{
double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number
@@ -151,8 +152,22 @@ double calcOverhangDistance(const ObjectData & object_data, const Pose & base_po
for (const auto & p : object_poly.outer()) {
const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point);
- largest_overhang = isOnRight(object_data) ? std::max(largest_overhang, lateral)
- : std::min(largest_overhang, lateral);
+
+ const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {
+ if (lateral > largest_overhang) {
+ overhang_pose = point;
+ }
+ return std::max(largest_overhang, lateral);
+ };
+
+ const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() {
+ if (lateral < largest_overhang) {
+ overhang_pose = point;
+ }
+ return std::min(largest_overhang, lateral);
+ };
+
+ largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left();
}
return largest_overhang;
}
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp
index e3b9eed8859ad..941e4b09fa0a3 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp
@@ -481,7 +481,82 @@ MarkerArray createPoseMarkerArray(
return msg;
}
+MarkerArray makeOverhangToRoadShoulderMarkerArray(
+ const behavior_path_planner::ObjectDataArray & objects)
+{
+ const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+ MarkerArray msg;
+
+ Marker marker{};
+ marker.header.frame_id = "map";
+ marker.header.stamp = current_time;
+ marker.ns = "overhang";
+
+ const auto normal_color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0);
+
+ int32_t i = 0;
+ for (const auto & object : objects) {
+ marker.id = i++;
+ marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+ marker.type = Marker::TEXT_VIEW_FACING;
+ // marker.action = Marker::ADD;
+ marker.pose = object.overhang_pose;
+ marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0);
+ marker.color = normal_color;
+ std::ostringstream string_stream;
+ string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])";
+ marker.text = string_stream.str();
+ msg.markers.push_back(marker);
+ }
+
+ return msg;
+}
+
+MarkerArray createOvehangFurthestLineStringMarkerArray(
+ const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
+ const double g, const double b)
+{
+ const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+ MarkerArray msg;
+ for (const auto & linestring : linestrings) {
+ Marker marker{};
+ marker.header.frame_id = "map";
+ marker.header.stamp = current_time;
+
+ marker.ns = ns;
+ marker.id = linestring.id();
+ marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+ marker.type = Marker::LINE_STRIP;
+ marker.action = Marker::ADD;
+ marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+ marker.scale = tier4_autoware_utils::createMarkerScale(0.4, 0.0, 0.0);
+ marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999);
+ for (const auto & p : linestring.basicLineString()) {
+ Point point;
+ point.x = p.x();
+ point.y = p.y();
+ point.z = p.z();
+ marker.points.push_back(point);
+ }
+ msg.markers.push_back(marker);
+ marker.ns = "linestring id";
+ marker.type = Marker::TEXT_VIEW_FACING;
+ Pose text_id_pose;
+ marker.scale = tier4_autoware_utils::createMarkerScale(1.5, 1.5, 1.5);
+ marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.8);
+ text_id_pose.position.x = linestring.front().x();
+ text_id_pose.position.y = linestring.front().y();
+ text_id_pose.position.z = linestring.front().z();
+ marker.pose = text_id_pose;
+ std::ostringstream ss;
+ ss << "(ID : " << linestring.id() << ") ";
+ marker.text = ss.str();
+ msg.markers.push_back(marker);
+ }
+
+ return msg;
+}
} // namespace marker_utils
std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr)