Skip to content

Commit 2bc22be

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

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
namespace
3439
{
3540
lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
@@ -78,44 +83,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
7883
return false;
7984
}
8085

81-
const auto current_lanes = getCurrentLanes();
82-
if (current_lanes.empty()) {
83-
RCLCPP_DEBUG(logger_, "no empty lanes");
84-
return false;
85-
}
86-
8786
const auto & nearest_object = data.target_objects.front();
87+
const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
88+
const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
8889

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

118-
return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
94+
return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length);
11995
}
12096

12197
bool AvoidanceByLaneChange::specialExpiredCheck() const
@@ -284,4 +260,48 @@ ObjectData AvoidanceByLaneChange::createObjectData(
284260

285261
return object_data;
286262
}
263+
264+
double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const
265+
{
266+
const auto ego_width = getCommonParam().vehicle_width;
267+
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
268+
const auto nearest_object_parameter =
269+
avoidance_parameters_->object_parameters.at(nearest_object_type);
270+
const auto avoid_margin =
271+
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
272+
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
273+
274+
avoidance_helper_->setData(planner_data_);
275+
const auto shift_length = avoidance_helper_->getShiftLength(
276+
nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
277+
278+
return avoidance_helper_->getMinAvoidanceDistance(shift_length);
279+
}
280+
281+
double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
282+
{
283+
const auto current_lanes = getCurrentLanes();
284+
if (current_lanes.empty()) {
285+
RCLCPP_DEBUG(logger_, "no empty lanes");
286+
return std::numeric_limits<double>::infinity();
287+
}
288+
289+
// get minimum lane change distance
290+
const auto shift_intervals =
291+
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
292+
return utils::lane_change::calcMinimumLaneChangeLength(
293+
*lane_change_parameters_, shift_intervals,
294+
lane_change_parameters_->backward_length_buffer_for_end_of_lane);
295+
}
296+
297+
double AvoidanceByLaneChange::calcLateralOffset() const
298+
{
299+
auto additional_lat_offset{0.0};
300+
for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
301+
const auto offset =
302+
2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
303+
additional_lat_offset = std::max(additional_lat_offset, offset);
304+
}
305+
return additional_lat_offset;
306+
}
287307
} // 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
@@ -1177,3 +1177,42 @@ rclcpp::Logger getLogger(const std::string & type)
11771177
return rclcpp::get_logger("lane_change").get_child(type);
11781178
}
11791179
} // namespace behavior_path_planner::utils::lane_change
1180+
1181+
namespace behavior_path_planner::utils::lane_change::debug
1182+
{
1183+
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
1184+
{
1185+
geometry_msgs::msg::Point32 p;
1186+
p.x = static_cast<float>(pose.position.x);
1187+
p.y = static_cast<float>(pose.position.y);
1188+
p.z = static_cast<float>(pose.position.z);
1189+
return p;
1190+
};
1191+
1192+
geometry_msgs::msg::Polygon createExecutionArea(
1193+
const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose,
1194+
double additional_lon_offset, double additional_lat_offset)
1195+
{
1196+
const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
1197+
const double & width = vehicle_info.vehicle_width_m;
1198+
const double & base_to_rear = vehicle_info.rear_overhang_m;
1199+
1200+
// if stationary object, extend forward and backward by the half of lon length
1201+
const double forward_lon_offset = base_to_front + additional_lon_offset;
1202+
const double backward_lon_offset = -base_to_rear;
1203+
const double lat_offset = width / 2.0 + additional_lat_offset;
1204+
1205+
const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
1206+
const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
1207+
const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
1208+
const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
1209+
geometry_msgs::msg::Polygon polygon;
1210+
1211+
polygon.points.push_back(create_point32(p1));
1212+
polygon.points.push_back(create_point32(p2));
1213+
polygon.points.push_back(create_point32(p3));
1214+
polygon.points.push_back(create_point32(p4));
1215+
1216+
return polygon;
1217+
}
1218+
} // namespace behavior_path_planner::utils::lane_change::debug

0 commit comments

Comments
 (0)