Skip to content

Commit

Permalink
add docstring
Browse files Browse the repository at this point in the history
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 committed Dec 23, 2024
1 parent f5aa74c commit 2647334
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -397,10 +397,37 @@ std::vector<lanelet::ConstLanelets> get_preceding_lanes(const CommonDataPtr & co
bool object_path_overlaps_lanes(
const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon);

/**
* @brief Converts a lane change path into multiple predicted paths with varying acceleration
* profiles.
*
* This function generates a set of predicted paths for the ego vehicle during a lane change,
* using different acceleration values within the specified range. It accounts for deceleration
* sampling if the global minimum acceleration differs from the lane-changing acceleration.
*
* @param common_data_ptr Shared pointer to CommonData containing parameters and state information.
* @param lane_change_path The lane change path used to generate predicted paths.
* @param deceleration_sampling_num Number of samples for deceleration profiles to generate paths.
*
* @return std::vector<std::vector<PoseWithVelocityStamped>> A collection of predicted paths, where
* each path is represented as a series of poses with associated velocity.
*/
std::vector<std::vector<PoseWithVelocityStamped>> convert_to_predicted_paths(
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
const size_t deceleration_sampling_num);

/**
* @brief Validates whether a given pose is a valid starting point for a lane change.
*
* This function checks if the specified pose lies within the polygons representing
* the target lane or its neighboring areas. This ensures that the starting point is
* appropriately positioned for initiating a lane change, even if previous paths were adjusted.
*
* @param common_data_ptr Shared pointer to CommonData containing lane polygon information.
* @param pose The pose to validate as a potential lane change starting point.
*
* @return true if the pose is within the target or target neighbor polygons, false otherwise.
*/
bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose);
} // namespace autoware::behavior_path_planner::utils::lane_change
#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -1127,6 +1127,7 @@ bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, co
const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor;
const auto & target_lane_poly = common_data_ptr->lanes_polygon_ptr->target;

// Check the target lane because the previous approved path might be shifted by avoidance module
return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) ||
boost::geometry::covered_by(lc_start_point, target_lane_poly);
}
Expand Down

0 comments on commit 2647334

Please sign in to comment.