|
23 | 23 | #include "rclcpp/logger.hpp"
|
24 | 24 |
|
25 | 25 | #include <route_handler/route_handler.hpp>
|
| 26 | +#include <tier4_autoware_utils/geometry/boost_geometry.hpp> |
26 | 27 |
|
27 | 28 | #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
|
28 | 29 | #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
|
|
31 | 32 |
|
32 | 33 | #include <lanelet2_core/Forward.h>
|
33 | 34 |
|
| 35 | +#include <memory> |
34 | 36 | #include <string>
|
35 | 37 | #include <vector>
|
36 | 38 |
|
@@ -185,7 +187,7 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
|
185 | 187 |
|
186 | 188 | ExtendedPredictedObject transform(
|
187 | 189 | const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
|
188 |
| - const LaneChangeParameters & lane_change_parameters); |
| 190 | + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); |
189 | 191 |
|
190 | 192 | bool isCollidedPolygonsInLanelet(
|
191 | 193 | const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
|
@@ -220,5 +222,72 @@ lanelet::ConstLanelets generateExpandedLanelets(
|
220 | 222 | * @return rclcpp::Logger The logger instance configured for the specified lane change type.
|
221 | 223 | */
|
222 | 224 | rclcpp::Logger getLogger(const std::string & type);
|
| 225 | + |
| 226 | +/** |
| 227 | + * @brief Computes the current footprint of the ego vehicle based on its pose and size. |
| 228 | + * |
| 229 | + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. |
| 230 | + * The footprint is determined by the vehicle's pose and its dimensions, including the distance |
| 231 | + * from the base to the front and rear ends of the vehicle, as well as its width. |
| 232 | + * |
| 233 | + * @param ego_pose The current pose of the ego vehicle. |
| 234 | + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal |
| 235 | + * offset, rear overhang, and width. |
| 236 | + * |
| 237 | + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. |
| 238 | + */ |
| 239 | +Polygon2d getEgoCurrentFootprint( |
| 240 | + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); |
| 241 | + |
| 242 | +/** |
| 243 | + * @brief Checks if the given polygon is within an intersection area. |
| 244 | + * |
| 245 | + * This function evaluates whether a specified polygon is located within the bounds of an |
| 246 | + * intersection. It identifies the intersection area by checking the attributes of the provided |
| 247 | + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function |
| 248 | + * then checks if the polygon is fully contained within this area. |
| 249 | + * |
| 250 | + * @param route_handler a shared pointer to the route_handler |
| 251 | + * @param lanelet A lanelet to check against the |
| 252 | + * intersection area. |
| 253 | + * @param polygon The polygon to check for containment within the intersection area. |
| 254 | + * |
| 255 | + * @return bool True if the polygon is within the intersection area, false otherwise. |
| 256 | + */ |
| 257 | +bool isWithinIntersection( |
| 258 | + const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet, |
| 259 | + const Polygon2d & polygon); |
| 260 | + |
| 261 | +/** |
| 262 | + * @brief Determines if a polygon is within lanes designated for turning. |
| 263 | + * |
| 264 | + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). |
| 265 | + * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's |
| 266 | + * area. |
| 267 | + * |
| 268 | + * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. |
| 269 | + * @param polygon The polygon to be checked for its presence within turn direction lanes. |
| 270 | + * |
| 271 | + * @return bool True if the polygon is within a lane designated for turning, false if it is within a |
| 272 | + * straight lane or no turn direction is specified. |
| 273 | + */ |
| 274 | +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); |
| 275 | + |
| 276 | +/** |
| 277 | + * @brief Calculates the distance required during a lane change operation. |
| 278 | + * |
| 279 | + * Used for computing prepare or lane change length based on current and maximum velocity, |
| 280 | + * acceleration, and duration, returning the lesser of accelerated distance or distance at max |
| 281 | + * velocity. |
| 282 | + * |
| 283 | + * @param velocity The current velocity of the vehicle in meters per second (m/s). |
| 284 | + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). |
| 285 | + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). |
| 286 | + * @param duration The duration of the lane change in seconds (s). |
| 287 | + * @return The calculated minimum distance in meters (m). |
| 288 | + */ |
| 289 | +double calcPhaseLength( |
| 290 | + const double velocity, const double maximum_velocity, const double acceleration, |
| 291 | + const double time); |
223 | 292 | } // namespace behavior_path_planner::utils::lane_change
|
224 | 293 | #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
|
0 commit comments