Skip to content

Commit 567541a

Browse files
fix(avoidance_by_lane_change): loosen execution condition
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent d381585 commit 567541a

File tree

9 files changed

+121
-42
lines changed

9 files changed

+121
-42
lines changed

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,10 @@ class AvoidanceByLaneChange : public NormalLaneChange
5454
const AvoidancePlanningData & data, const PredictedObject & object) const;
5555

5656
void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
57+
58+
double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
59+
double calcMinimumLaneChangeLength() const;
60+
double calcLateralOffset() const;
5761
};
5862
} // namespace behavior_path_planner
5963

planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,9 @@
1616

1717
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
1818
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
19-
#include "behavior_path_planner_common/marker_utils/utils.hpp"
2019

21-
#include <algorithm>
2220
#include <memory>
2321
#include <string>
24-
#include <utility>
25-
#include <vector>
2622

2723
namespace behavior_path_planner
2824
{
@@ -45,7 +41,8 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
4541

4642
bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
4743
{
48-
return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck();
44+
return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
45+
module_type_->isValidPath();
4946
}
5047
void AvoidanceByLaneChangeInterface::processOnEntry()
5148
{

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+55-35
Original file line numberDiff line numberDiff line change
@@ -20,16 +20,21 @@
2020
#include "behavior_path_planner_common/utils/path_utils.hpp"
2121
#include "behavior_path_planner_common/utils/utils.hpp"
2222

23+
#include <behavior_path_avoidance_module/data_structs.hpp>
24+
#include <behavior_path_lane_change_module/utils/utils.hpp>
2325
#include <lanelet2_extension/utility/utilities.hpp>
2426
#include <rclcpp/logging.hpp>
2527

2628
#include <boost/geometry/algorithms/centroid.hpp>
2729
#include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
2830

31+
#include <limits>
2932
#include <utility>
3033

3134
namespace behavior_path_planner
3235
{
36+
using behavior_path_planner::utils::lane_change::debug::createExecutionArea;
37+
3338
AvoidanceByLaneChange::AvoidanceByLaneChange(
3439
const std::shared_ptr<LaneChangeParameters> & parameters,
3540
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
@@ -68,44 +73,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
6873
return false;
6974
}
7075

71-
const auto current_lanes = getCurrentLanes();
72-
if (current_lanes.empty()) {
73-
RCLCPP_DEBUG(logger_, "no empty lanes");
74-
return false;
75-
}
76-
7776
const auto & nearest_object = data.target_objects.front();
77+
const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
78+
const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
7879

79-
// get minimum lane change distance
80-
const auto shift_intervals =
81-
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
82-
const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
83-
*lane_change_parameters_, shift_intervals,
84-
lane_change_parameters_->backward_length_buffer_for_end_of_lane);
85-
86-
// get minimum avoid distance
87-
88-
const auto ego_width = getCommonParam().vehicle_width;
89-
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
90-
const auto nearest_object_parameter =
91-
avoidance_parameters_->object_parameters.at(nearest_object_type);
92-
const auto avoid_margin =
93-
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
94-
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
95-
96-
avoidance_helper_->setData(planner_data_);
97-
const auto shift_length = avoidance_helper_->getShiftLength(
98-
nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
99-
100-
const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);
101-
102-
RCLCPP_DEBUG(
103-
logger_,
104-
"nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance "
105-
"%.3f",
106-
nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance);
80+
lane_change_debug_.execution_area = createExecutionArea(
81+
getCommonParam().vehicle_info, getEgoPose(),
82+
std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());
10783

108-
return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
84+
return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length);
10985
}
11086

11187
bool AvoidanceByLaneChange::specialExpiredCheck() const
@@ -274,4 +250,48 @@ ObjectData AvoidanceByLaneChange::createObjectData(
274250

275251
return object_data;
276252
}
253+
254+
double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const
255+
{
256+
const auto ego_width = getCommonParam().vehicle_width;
257+
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
258+
const auto nearest_object_parameter =
259+
avoidance_parameters_->object_parameters.at(nearest_object_type);
260+
const auto avoid_margin =
261+
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
262+
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
263+
264+
avoidance_helper_->setData(planner_data_);
265+
const auto shift_length = avoidance_helper_->getShiftLength(
266+
nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
267+
268+
return avoidance_helper_->getMinAvoidanceDistance(shift_length);
269+
}
270+
271+
double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
272+
{
273+
const auto current_lanes = getCurrentLanes();
274+
if (current_lanes.empty()) {
275+
RCLCPP_DEBUG(logger_, "no empty lanes");
276+
return std::numeric_limits<double>::infinity();
277+
}
278+
279+
// get minimum lane change distance
280+
const auto shift_intervals =
281+
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
282+
return utils::lane_change::calcMinimumLaneChangeLength(
283+
*lane_change_parameters_, shift_intervals,
284+
lane_change_parameters_->backward_length_buffer_for_end_of_lane);
285+
}
286+
287+
double AvoidanceByLaneChange::calcLateralOffset() const
288+
{
289+
auto additional_lat_offset{0.0};
290+
for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
291+
const auto offset =
292+
2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
293+
additional_lat_offset = std::max(additional_lat_offset, offset);
294+
}
295+
return additional_lat_offset;
296+
}
277297
} // namespace behavior_path_planner

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919

2020
#include <behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp>
2121

22+
#include <geometry_msgs/msg/detail/polygon__struct.hpp>
23+
2224
#include <string>
2325

2426
namespace behavior_path_planner::data::lane_change
@@ -31,6 +33,7 @@ struct Debug
3133
CollisionCheckDebugMap collision_check_objects;
3234
CollisionCheckDebugMap collision_check_objects_after_approval;
3335
LaneChangeTargetObjects filtered_objects;
36+
geometry_msgs::msg::Polygon execution_area;
3437
double collision_check_object_debug_lifetime{0.0};
3538

3639
void reset()

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include "behavior_path_lane_change_module/utils/path.hpp"
2020
#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
2121

22+
#include <geometry_msgs/msg/detail/polygon__struct.hpp>
2223
#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
2324
#include <visualization_msgs/msg/marker_array.hpp>
2425

@@ -40,6 +41,8 @@ MarkerArray showFilteredObjects(
4041
const ExtendedPredictedObjects & current_lane_objects,
4142
const ExtendedPredictedObjects & target_lane_objects,
4243
const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
44+
MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
4345
MarkerArray createDebugMarkerArray(const Debug & debug_data);
46+
4447
} // namespace marker_utils::lane_change_markers
4548
#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

+10
Original file line numberDiff line numberDiff line change
@@ -225,4 +225,14 @@ lanelet::ConstLanelets generateExpandedLanelets(
225225
*/
226226
rclcpp::Logger getLogger(const std::string & type);
227227
} // namespace behavior_path_planner::utils::lane_change
228+
229+
namespace behavior_path_planner::utils::lane_change::debug
230+
{
231+
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose);
232+
233+
geometry_msgs::msg::Polygon createExecutionArea(
234+
const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose,
235+
double additional_lon_offset, double additional_lat_offset);
236+
} // namespace behavior_path_planner::utils::lane_change::debug
237+
228238
#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_lane_change_module/src/scene.cpp

-2
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515
#include "behavior_path_lane_change_module/scene.hpp"
1616

1717
#include "behavior_path_lane_change_module/utils/utils.hpp"
18-
#include "behavior_path_planner_common/marker_utils/utils.hpp"
1918
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
2019
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
2120
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
@@ -33,7 +32,6 @@
3332
#include <algorithm>
3433
#include <limits>
3534
#include <memory>
36-
#include <string>
3735
#include <utility>
3836
#include <vector>
3937

planning/behavior_path_lane_change_module/src/utils/markers.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -162,6 +162,11 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data)
162162
"ego_and_target_polygon_relation_after_approval"));
163163
}
164164

165+
if (!debug_data.execution_area.points.empty()) {
166+
add(createPolygonMarkerArray(
167+
debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1));
168+
}
169+
165170
return debug_marker;
166171
}
167172
} // namespace marker_utils::lane_change_markers

planning/behavior_path_lane_change_module/src/utils/utils.cpp

+39
Original file line numberDiff line numberDiff line change
@@ -1193,3 +1193,42 @@ rclcpp::Logger getLogger(const std::string & type)
11931193
return rclcpp::get_logger("lane_change").get_child(type);
11941194
}
11951195
} // namespace behavior_path_planner::utils::lane_change
1196+
1197+
namespace behavior_path_planner::utils::lane_change::debug
1198+
{
1199+
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
1200+
{
1201+
geometry_msgs::msg::Point32 p;
1202+
p.x = static_cast<float>(pose.position.x);
1203+
p.y = static_cast<float>(pose.position.y);
1204+
p.z = static_cast<float>(pose.position.z);
1205+
return p;
1206+
};
1207+
1208+
geometry_msgs::msg::Polygon createExecutionArea(
1209+
const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose,
1210+
double additional_lon_offset, double additional_lat_offset)
1211+
{
1212+
const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
1213+
const double & width = vehicle_info.vehicle_width_m;
1214+
const double & base_to_rear = vehicle_info.rear_overhang_m;
1215+
1216+
// if stationary object, extend forward and backward by the half of lon length
1217+
const double forward_lon_offset = base_to_front + additional_lon_offset;
1218+
const double backward_lon_offset = -base_to_rear;
1219+
const double lat_offset = width / 2.0 + additional_lat_offset;
1220+
1221+
const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
1222+
const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
1223+
const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
1224+
const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
1225+
geometry_msgs::msg::Polygon polygon;
1226+
1227+
polygon.points.push_back(create_point32(p1));
1228+
polygon.points.push_back(create_point32(p2));
1229+
polygon.points.push_back(create_point32(p3));
1230+
polygon.points.push_back(create_point32(p4));
1231+
1232+
return polygon;
1233+
}
1234+
} // namespace behavior_path_planner::utils::lane_change::debug

0 commit comments

Comments
 (0)