diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index f30ed9d7a68da..0dee17bd0ef03 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -243,7 +243,11 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto recordTime(2); // Calculate stop point with margin - const auto default_stop_pose = getDefaultStopPose(*path, first_path_point_on_crosswalk); + const auto default_stop_pose_opt = getDefaultStopPose(*path, first_path_point_on_crosswalk); + if (!default_stop_pose_opt) { + RCLCPP_DEBUG(logger_, "Failure to get default_stop_pose"); + return {}; + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -253,12 +257,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Decide to stop for crosswalk users const auto stop_factor_for_crosswalk_users = checkStopForCrosswalkUsers( *path, sparse_resample_path, first_path_point_on_crosswalk, last_path_point_on_crosswalk, - default_stop_pose); + *default_stop_pose_opt); // Decide to stop for stuck vehicle const auto stop_factor_for_stuck_vehicles = checkStopForStuckVehicles( sparse_resample_path, objects_ptr->objects, first_path_point_on_crosswalk, - last_path_point_on_crosswalk, default_stop_pose); + last_path_point_on_crosswalk, *default_stop_pose_opt); // Get nearest stop factor const auto nearest_stop_factor = @@ -270,13 +274,13 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Set distance // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated. - setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); + setDistanceToStop(*path, *default_stop_pose_opt, nearest_stop_factor); // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); } else { - planStop(*path, nearest_stop_factor, default_stop_pose, stop_reason); + planStop(*path, nearest_stop_factor, *default_stop_pose_opt, stop_reason); } recordTime(4); @@ -314,7 +318,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & default_stop_pose) + const geometry_msgs::msg::Pose & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; const double ego_vel = planner_data_->current_velocity->twist.linear.x; @@ -322,7 +326,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = - calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); + calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose.position); // Calculate attention range for crosswalk const auto crosswalk_attention_range = getAttentionRange( @@ -400,9 +404,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if (stop_at_stop_line) { // Stop at the stop line - if (default_stop_pose) { - return createStopFactor(*default_stop_pose, stop_factor_points); - } + return createStopFactor(default_stop_pose, stop_factor_points); } else { const auto stop_pose = calcLongitudinalOffsetPose( ego_path.points, nearest_stop_info->first, @@ -946,14 +948,10 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & stop_pose) + const geometry_msgs::msg::Pose & stop_pose) { const auto & p = planner_param_; - if (!stop_pose) { - return {}; - } - // skip stuck vehicle checking for crosswalk which is in intersection. if (!p.enable_stuck_check_in_intersection) { std::string turn_direction = road_.attributeOr("turn_direction", "else"); @@ -1004,7 +1002,7 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); + calcSignedArcLength(ego_path.points, ego_pos, stop_pose.position); const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); @@ -1246,15 +1244,13 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( } void CrosswalkModule::setDistanceToStop( - const PathWithLaneId & ego_path, - const std::optional & default_stop_pose, + const PathWithLaneId & ego_path, const geometry_msgs::msg::Pose & default_stop_pose, const std::optional & stop_factor) { // calculate stop position const auto stop_pos = [&]() -> std::optional { if (stop_factor) return stop_factor->stop_pose.position; - if (default_stop_pose) return default_stop_pose->position; - return std::nullopt; + return default_stop_pose.position; }(); // Set distance @@ -1282,12 +1278,11 @@ void CrosswalkModule::planGo( void CrosswalkModule::planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason) + const geometry_msgs::msg::Pose & default_stop_pose, StopReason * stop_reason) { const auto stop_factor = [&]() -> std::optional { if (nearest_stop_factor) return *nearest_stop_factor; - if (default_stop_pose) return createStopFactor(*default_stop_pose); - return std::nullopt; + return createStopFactor(default_stop_pose); }(); if (!stop_factor) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ebb9d715ccd6a..3496eb6795c25 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -352,13 +352,13 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & default_stop_pose); + const geometry_msgs::msg::Pose & default_stop_pose); std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & stop_pose); + const geometry_msgs::msg::Pose & stop_pose); std::optional findEgoPassageDirectionAlongPath( const PathWithLaneId & sparse_resample_path) const; @@ -376,14 +376,14 @@ class CrosswalkModule : public SceneModuleInterface void setDistanceToStop( const PathWithLaneId & ego_path, - const std::optional & default_stop_pose, + const geometry_msgs::msg::Pose & default_stop_pose, const std::optional & stop_factor); void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor) const; void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, - const std::optional & default_stop_pose, StopReason * stop_reason); + const geometry_msgs::msg::Pose & default_stop_pose, StopReason * stop_reason); // minor functions std::pair getAttentionRange(