Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(avoidance_by_lane_change): loosen execution condition #6220

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,10 @@ class AvoidanceByLaneChange : public NormalLaneChange
const AvoidancePlanningData & data, const PredictedObject & object) const;

void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;

double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
double calcMinimumLaneChangeLength() const;
double calcLateralOffset() const;
};
} // namespace behavior_path_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,9 @@

#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
#include "behavior_path_planner_common/marker_utils/utils.hpp"

#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>

namespace behavior_path_planner
{
Expand All @@ -45,7 +41,8 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(

bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
{
return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck();
return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
module_type_->isValidPath();
}
void AvoidanceByLaneChangeInterface::processOnEntry()
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,21 @@
#include "behavior_path_planner_common/utils/path_utils.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"

#include <behavior_path_avoidance_module/data_structs.hpp>
#include <behavior_path_lane_change_module/utils/utils.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <rclcpp/logging.hpp>

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

#include <limits>
#include <utility>

namespace behavior_path_planner
{
using behavior_path_planner::utils::lane_change::debug::createExecutionArea;

AvoidanceByLaneChange::AvoidanceByLaneChange(
const std::shared_ptr<LaneChangeParameters> & parameters,
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
Expand Down Expand Up @@ -68,44 +73,15 @@
return false;
}

const auto current_lanes = getCurrentLanes();
if (current_lanes.empty()) {
RCLCPP_DEBUG(logger_, "no empty lanes");
return false;
}

const auto & nearest_object = data.target_objects.front();
const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
const auto minimum_lane_change_length = calcMinimumLaneChangeLength();

Check warning on line 78 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L77-L78

Added lines #L77 - L78 were not covered by tests

// get minimum lane change distance
const auto shift_intervals =
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
*lane_change_parameters_, shift_intervals,
lane_change_parameters_->backward_length_buffer_for_end_of_lane);

// get minimum avoid distance

const auto ego_width = getCommonParam().vehicle_width;
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
const auto nearest_object_parameter =
avoidance_parameters_->object_parameters.at(nearest_object_type);
const auto avoid_margin =
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;

avoidance_helper_->setData(planner_data_);
const auto shift_length = avoidance_helper_->getShiftLength(
nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);

const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);

RCLCPP_DEBUG(
logger_,
"nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance "
"%.3f",
nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance);
lane_change_debug_.execution_area = createExecutionArea(
getCommonParam().vehicle_info, getEgoPose(),

Check warning on line 81 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L80-L81

Added lines #L80 - L81 were not covered by tests
std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());

return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length);

Check warning on line 84 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L84

Added line #L84 was not covered by tests
}

bool AvoidanceByLaneChange::specialExpiredCheck() const
Expand Down Expand Up @@ -274,4 +250,48 @@

return object_data;
}

double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const

Check warning on line 254 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L254

Added line #L254 was not covered by tests
{
const auto ego_width = getCommonParam().vehicle_width;
const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);

Check warning on line 257 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L256-L257

Added lines #L256 - L257 were not covered by tests
const auto nearest_object_parameter =
avoidance_parameters_->object_parameters.at(nearest_object_type);
const auto avoid_margin =
nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;

Check warning on line 262 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L259-L262

Added lines #L259 - L262 were not covered by tests

avoidance_helper_->setData(planner_data_);
const auto shift_length = avoidance_helper_->getShiftLength(
nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);

Check warning on line 266 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L265-L266

Added lines #L265 - L266 were not covered by tests

return avoidance_helper_->getMinAvoidanceDistance(shift_length);

Check warning on line 268 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L268

Added line #L268 was not covered by tests
}

double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const

Check warning on line 271 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L271

Added line #L271 was not covered by tests
{
const auto current_lanes = getCurrentLanes();

Check warning on line 273 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L273

Added line #L273 was not covered by tests
if (current_lanes.empty()) {
RCLCPP_DEBUG(logger_, "no empty lanes");
return std::numeric_limits<double>::infinity();
}

// get minimum lane change distance
const auto shift_intervals =
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
return utils::lane_change::calcMinimumLaneChangeLength(
*lane_change_parameters_, shift_intervals,
lane_change_parameters_->backward_length_buffer_for_end_of_lane);
}

Check warning on line 285 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L285

Added line #L285 was not covered by tests

double AvoidanceByLaneChange::calcLateralOffset() const

Check warning on line 287 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L287

Added line #L287 was not covered by tests
{
auto additional_lat_offset{0.0};

Check warning on line 289 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L289

Added line #L289 was not covered by tests
for (const auto & [type, p] : avoidance_parameters_->object_parameters) {
const auto offset =

Check warning on line 291 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L291

Added line #L291 was not covered by tests
2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
additional_lat_offset = std::max(additional_lat_offset, offset);

Check warning on line 293 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L293

Added line #L293 was not covered by tests
}
return additional_lat_offset;

Check warning on line 295 in planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp#L295

Added line #L295 was not covered by tests
}
} // namespace behavior_path_planner
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@

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

#include <geometry_msgs/msg/detail/polygon__struct.hpp>

#include <string>

namespace behavior_path_planner::data::lane_change
Expand All @@ -31,6 +33,7 @@ struct Debug
CollisionCheckDebugMap collision_check_objects;
CollisionCheckDebugMap collision_check_objects_after_approval;
LaneChangeTargetObjects filtered_objects;
geometry_msgs::msg::Polygon execution_area;
double collision_check_object_debug_lifetime{0.0};

void reset()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "behavior_path_lane_change_module/utils/path.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"

#include <geometry_msgs/msg/detail/polygon__struct.hpp>
#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand All @@ -40,6 +41,8 @@ MarkerArray showFilteredObjects(
const ExtendedPredictedObjects & current_lane_objects,
const ExtendedPredictedObjects & target_lane_objects,
const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
MarkerArray createDebugMarkerArray(const Debug & debug_data);

} // namespace marker_utils::lane_change_markers
#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -225,4 +225,14 @@ lanelet::ConstLanelets generateExpandedLanelets(
*/
rclcpp::Logger getLogger(const std::string & type);
} // namespace behavior_path_planner::utils::lane_change

namespace behavior_path_planner::utils::lane_change::debug
{
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose);

geometry_msgs::msg::Polygon createExecutionArea(
const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose,
double additional_lon_offset, double additional_lat_offset);
} // namespace behavior_path_planner::utils::lane_change::debug

#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
2 changes: 0 additions & 2 deletions planning/behavior_path_lane_change_module/src/scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#include "behavior_path_lane_change_module/scene.hpp"

#include "behavior_path_lane_change_module/utils/utils.hpp"
#include "behavior_path_planner_common/marker_utils/utils.hpp"
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
Expand All @@ -33,7 +32,6 @@
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,11 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data)
"ego_and_target_polygon_relation_after_approval"));
}

if (!debug_data.execution_area.points.empty()) {
add(createPolygonMarkerArray(
debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1));
}

return debug_marker;
}
} // namespace marker_utils::lane_change_markers
39 changes: 39 additions & 0 deletions planning/behavior_path_lane_change_module/src/utils/utils.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity

The mean cyclomatic complexity decreases from 4.86 to 4.67, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.

Check notice on line 1 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession

The ratio of primitive types in function arguments increases from 34.07% to 34.29%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -1193,3 +1193,42 @@
return rclcpp::get_logger("lane_change").get_child(type);
}
} // namespace behavior_path_planner::utils::lane_change

namespace behavior_path_planner::utils::lane_change::debug
{
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)

Check warning on line 1199 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1199

Added line #L1199 was not covered by tests
{
geometry_msgs::msg::Point32 p;
p.x = static_cast<float>(pose.position.x);
p.y = static_cast<float>(pose.position.y);
p.z = static_cast<float>(pose.position.z);
return p;

Check warning on line 1205 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1202-L1205

Added lines #L1202 - L1205 were not covered by tests
};

geometry_msgs::msg::Polygon createExecutionArea(

Check warning on line 1208 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1208

Added line #L1208 was not covered by tests
const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose,
double additional_lon_offset, double additional_lat_offset)
{
const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
const double & width = vehicle_info.vehicle_width_m;
const double & base_to_rear = vehicle_info.rear_overhang_m;

// if stationary object, extend forward and backward by the half of lon length
const double forward_lon_offset = base_to_front + additional_lon_offset;
const double backward_lon_offset = -base_to_rear;
const double lat_offset = width / 2.0 + additional_lat_offset;

Check warning on line 1219 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1217-L1219

Added lines #L1217 - L1219 were not covered by tests

const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);

Check warning on line 1224 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1221-L1224

Added lines #L1221 - L1224 were not covered by tests
geometry_msgs::msg::Polygon polygon;

polygon.points.push_back(create_point32(p1));
polygon.points.push_back(create_point32(p2));
polygon.points.push_back(create_point32(p3));
polygon.points.push_back(create_point32(p4));

return polygon;

Check warning on line 1232 in planning/behavior_path_lane_change_module/src/utils/utils.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/utils/utils.cpp#L1232

Added line #L1232 was not covered by tests
}
} // namespace behavior_path_planner::utils::lane_change::debug
Loading