Skip to content

Commit

Permalink
feat(lane_change): modify lane change target boundary check to consid…
Browse files Browse the repository at this point in the history
…er velocity (#8961)

* check if candidate path footprint exceeds target lane boundary when lc velocity is above minimum

Signed-off-by: mohammad alqudah <[email protected]>

* move functions to relevant module

Signed-off-by: mohammad alqudah <[email protected]>

* suppress unused function cppcheck

Signed-off-by: mohammad alqudah <[email protected]>

* minor change

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
  • Loading branch information
mkquda authored Sep 27, 2024
1 parent f0b43d6 commit 68abc99
Show file tree
Hide file tree
Showing 7 changed files with 66 additions and 75 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
{
}

// cppcheck-suppress unusedFunction
bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
{
return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
module_type_->isValidPath();
}

// cppcheck-suppress unusedFunction
void AvoidanceByLaneChangeInterface::processOnEntry()
{
waitApproval();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -186,8 +186,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);
}

std::unique_ptr<SceneModuleInterface>
AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
// cppcheck-suppress unusedFunction
SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
{
return std::make_unique<AvoidanceByLaneChangeInterface>(
name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager;
using autoware::behavior_path_planner::LaneChangeModuleType;
using autoware::behavior_path_planner::SceneModuleInterface;

using SMIPtr = std::unique_ptr<SceneModuleInterface>;

class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
{
public:
Expand All @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager

void init(rclcpp::Node * node) override;

std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
SMIPtr createNewSceneModuleInstance() override;

private:
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,13 +34,55 @@
#include <optional>
#include <utility>

namespace
{
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
{
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;
};

geometry_msgs::msg::Polygon create_execution_area(
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
const geometry_msgs::msg::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;

const auto p1 =
autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
const auto p2 =
autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
const auto p3 =
autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
const auto p4 =
autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
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;
}
} // namespace

namespace autoware::behavior_path_planner
{
using autoware::behavior_path_planner::Direction;
using autoware::behavior_path_planner::LaneChangeModuleType;
using autoware::behavior_path_planner::ObjectInfo;
using autoware::behavior_path_planner::Point2d;
using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea;

AvoidanceByLaneChange::AvoidanceByLaneChange(
const std::shared_ptr<LaneChangeParameters> & parameters,
Expand Down Expand Up @@ -84,7 +126,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
const auto minimum_lane_change_length = calcMinimumLaneChangeLength();

lane_change_debug_.execution_area = createExecutionArea(
lane_change_debug_.execution_area = create_execution_area(
getCommonParam().vehicle_info, getEgoPose(),
std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ bool isPathInLanelets(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);

bool pathFootprintExceedsTargetLaneBound(
bool path_footprint_exceeds_target_lane_bound(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
const double margin = 0.1);

Expand Down Expand Up @@ -257,10 +257,6 @@ bool is_same_lane_with_prev_iteration(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);

Pose to_pose(
const autoware::universe_utils::Point2d & point,
const geometry_msgs::msg::Quaternion & orientation);

bool is_ahead_of_ego(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object);
Expand All @@ -280,13 +276,4 @@ double get_distance_to_next_regulatory_element(
const bool ignore_intersection = false);
} // namespace autoware::behavior_path_planner::utils::lane_change

namespace autoware::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 VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset,
double additional_lat_offset);
} // namespace autoware::behavior_path_planner::utils::lane_change::debug

#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -1609,6 +1609,18 @@ bool NormalLaneChange::check_candidate_path_safety(
"Ego is not stuck and parked vehicle exists in the target lane. Skip lane change.");
}

const auto lc_start_velocity = candidate_path.info.velocity.prepare;
const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity;
constexpr double margin = 0.1;
// path is unsafe if it exceeds target lane boundary with a high velocity
if (
lane_change_parameters_->enable_target_lane_bound_check &&
lc_start_velocity > min_lc_velocity + margin &&
utils::lane_change::path_footprint_exceeds_target_lane_bound(
common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) {
throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change.");
}

constexpr size_t decel_sampling_num = 1;
const auto safety_check_with_normal_rss = isLaneChangePathSafe(
candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -200,8 +200,7 @@ bool isPathInLanelets(
return true;
}

// cppcheck-suppress unusedFunction
bool pathFootprintExceedsTargetLaneBound(
bool path_footprint_exceeds_target_lane_bound(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
const double margin)
{
Expand Down Expand Up @@ -1145,16 +1144,6 @@ bool is_same_lane_with_prev_iteration(
(prev_target_lanes.back().id() == prev_target_lanes.back().id());
}

Pose to_pose(
const autoware::universe_utils::Point2d & point,
const geometry_msgs::msg::Quaternion & orientation)
{
Pose pose;
pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0);
pose.orientation = orientation;
return pose;
}

bool is_ahead_of_ego(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object)
Expand Down Expand Up @@ -1279,47 +1268,3 @@ double get_distance_to_next_regulatory_element(
return distance;
}
} // namespace autoware::behavior_path_planner::utils::lane_change

namespace autoware::behavior_path_planner::utils::lane_change::debug
{
geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
{
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;
};

geometry_msgs::msg::Polygon createExecutionArea(
const autoware::vehicle_info_utils::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;

const auto p1 =
autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
const auto p2 =
autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
const auto p3 =
autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
const auto p4 =
autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
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;
}

} // namespace autoware::behavior_path_planner::utils::lane_change::debug

0 comments on commit 68abc99

Please sign in to comment.