diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index e7accc14096e0..470f37f2430a0 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -17,6 +17,8 @@ far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + # If the ahead stop position is already inserted, use the same position for the new stop point. + max_ahead_longitudinal_margin: 5.0 # [m] specifies the margin between the current position and the stop point # param for ego's slow down velocity slow_down: diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 94e87d0174193..2775c694b15d2 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -53,6 +53,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + cp.max_ahead_longitudinal_margin = + getOrDeclareParameter(node, ns + ".stop_position.max_ahead_longitudinal_margin"); // param for ego velocity cp.min_slow_down_velocity = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 62acd191d8697..dfa5f55441817 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -313,6 +313,39 @@ std::optional> CrosswalkModule::get return {}; } +std::vector CrosswalkModule::searchAheadInsertedStopPoint( + const PathWithLaneId & ego_path, const Point & candidate_stop_point, + const double ahead_margin) const +{ + // If the stop point is not found, return empty vector. + std::vector inserted_forward_stop_point{}; + + const double epsilon = 1e-3; + const int candidate_stop_point_idx = + findNearestSegmentIndex(ego_path.points, candidate_stop_point); + + // Start from the segment just before the nearest or from 0 if it's the first segment. + size_t start_idx = std::max(candidate_stop_point_idx - 1, 0); + for (size_t idx = start_idx; idx < ego_path.points.size() - 1; ++idx) { + if (calcSignedArcLength(ego_path.points, start_idx, idx) > ahead_margin) { + break; + } + if (std::abs(ego_path.points.at(idx).point.longitudinal_velocity_mps) < epsilon) { + RCLCPP_DEBUG_THROTTLE( + logger_, *clock_, 500, + "Stop point is found and distance from inserted stop point: %f module_id: %ld", + calcSignedArcLength(ego_path.points, candidate_stop_point_idx, idx), module_id_); + const auto stop_point = createPoint( + ego_path.points.at(idx).point.pose.position.x, + ego_path.points.at(idx).point.pose.position.y, + ego_path.points.at(idx).point.pose.position.z); + inserted_forward_stop_point.push_back(stop_point); + break; + } + } + return inserted_forward_stop_point; +} + std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const std::optional> & p_stop_line, @@ -383,17 +416,51 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if (!nearest_stop_info) { return {}; } + std::vector inserted_forward_stop_point{}; + bool merged_stop_point_is_front_of_crosswalk = false; + + if (default_stop_pose.has_value()) { + const double ahead_margin = planner_param_.max_ahead_longitudinal_margin; + const Point default_stop_point = createPoint( + default_stop_pose->position.x, default_stop_pose->position.y, default_stop_pose->position.z); + // Search for the inserted stop point that is ahead of the ego path with the margin. + inserted_forward_stop_point = + searchAheadInsertedStopPoint(ego_path, default_stop_point, ahead_margin); + + // If there are any inserted forward stop points + if (!inserted_forward_stop_point.empty()) { + const double dist_inserted_stop_point2crosswalk = calcSignedArcLength( + ego_path.points, path_intersects.front(), inserted_forward_stop_point.front()); + // Check if the merged stop point is in front of the crosswalk + merged_stop_point_is_front_of_crosswalk = + dist_inserted_stop_point2crosswalk + base_link2front < 0.0; + RCLCPP_DEBUG_THROTTLE( + logger_, *clock_, 500, "Distance to crosswalk: %f", dist_inserted_stop_point2crosswalk); + if (!merged_stop_point_is_front_of_crosswalk) { + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 500, "Stop point is behind crosswalk"); + } + } + } // Check if the ego should stop beyond the stop line. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; - if (stop_at_stop_line) { - // Stop at the stop line - if (default_stop_pose) { - return createStopFactor(*default_stop_pose, stop_factor_points); + RCLCPP_DEBUG(logger_, "stop_at_stop_line: %d", stop_at_stop_line); + RCLCPP_DEBUG(logger_, "default_stop_pose: %d", default_stop_pose.has_value()); + RCLCPP_DEBUG( + logger_, "merged_stop_point_is_front_of_crosswalk: %d", + merged_stop_point_is_front_of_crosswalk); + + if (stop_at_stop_line && default_stop_pose) { + if (merged_stop_point_is_front_of_crosswalk) { + RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 500, "merge with forward stop point"); + const auto inserted_stop_pose = + calcLongitudinalOffsetPose(ego_path.points, inserted_forward_stop_point.front(), 0.0); + return createStopFactor(*inserted_stop_pose, inserted_forward_stop_point); } + return createStopFactor(*default_stop_pose, stop_factor_points); } else { // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 063ea4f83cd45..c9e3950d837a7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -54,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::TrafficSignalElement; +using geometry_msgs::msg::Point; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; @@ -115,6 +116,7 @@ class CrosswalkModule : public SceneModuleInterface double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + double max_ahead_longitudinal_margin; // param for ego velocity float min_slow_down_velocity; double max_slow_down_jerk; @@ -160,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; - geometry_msgs::msg::Point position{}; + Point position{}; std::optional collision_point{}; void transitState( @@ -311,21 +313,20 @@ class CrosswalkModule : public SceneModuleInterface private: // main functions void applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects); + PathWithLaneId & output, const std::vector & path_intersects); - std::optional> getStopPointWithMargin( - const PathWithLaneId & ego_path, - const std::vector & path_intersects) const; + std::optional> getStopPointWithMargin( + const PathWithLaneId & ego_path, const std::vector & path_intersects) const; std::optional checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, - const std::optional> & p_stop_line, - const std::vector & path_intersects, + const std::optional> & p_stop_line, + const std::vector & path_intersects, const std::optional & default_stop_pose); std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, - const std::vector & path_intersects, + const std::vector & path_intersects, const std::optional & stop_pose) const; std::optional getCollisionPoint( @@ -337,6 +338,28 @@ class CrosswalkModule : public SceneModuleInterface const std::optional & stop_factor_for_crosswalk_users, const std::optional & stop_factor_for_stuck_vehicles); + /** + * @brief Searches for stop points ahead of a given candidate stop point in the ego path. + * + * This function searches for stop points that are ahead of the given candidate stop point + * within a specified margin. If a stop point is found within the margin and its longitudinal + * velocity is approximately zero, it is considered as a valid stop point and added to the result. + * If no such point is found, an empty vector is returned. + * + * @param ego_path The path of the ego vehicle witch contains velocity information. + * @param candidate_stop_point The reference point to start searching for stop points. + * @param ahead_margin The distance margin ahead of the candidate stop point within which to + * search for stop points. + * @return A vector containing the found stop points. If no stop point is found, returns an empty + * vector. + * + * @note The function uses a small epsilon value to check if the longitudinal velocity of a point + * is approximately zero. + */ + std::vector searchAheadInsertedStopPoint( + const PathWithLaneId & ego_path, const Point & candidate_stop_point, + const double ahead_margin) const; + void setDistanceToStop( const PathWithLaneId & ego_path, const std::optional & default_stop_pose, @@ -350,24 +373,20 @@ class CrosswalkModule : public SceneModuleInterface // minor functions std::pair getAttentionRange( - const PathWithLaneId & ego_path, - const std::vector & path_intersects); + const PathWithLaneId & ego_path, const std::vector & path_intersects); void insertDecelPointWithDebugInfo( - const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output) const; + const Point & stop_point, const float target_velocity, PathWithLaneId & output) const; std::pair clampAttentionRangeByNeighborCrosswalks( const PathWithLaneId & ego_path, const double near_attention_range, const double far_attention_range); CollisionPoint createCollisionPoint( - const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, - const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const; + const Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, + const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const; - float calcTargetVelocity( - const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; + float calcTargetVelocity(const Point & stop_point, const PathWithLaneId & ego_path) const; Polygon2d getAttentionArea( const PathWithLaneId & sparse_resample_path, @@ -375,7 +394,7 @@ class CrosswalkModule : public SceneModuleInterface bool isStuckVehicle( const PathWithLaneId & ego_path, const std::vector & objects, - const std::vector & path_intersects) const; + const std::vector & path_intersects) const; void updateObjectState( const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,