diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index b1e5bcb32bd18..e7accc14096e0 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -54,7 +54,9 @@ ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for target object filtering diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 304b4bdf6f5e7..94e87d0174193 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -104,8 +104,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.timeout_no_intention_to_walk = - getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); + cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_map_for_no_intention_to_walk"); + cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 6d2b610ed0ef7..a44d4a898c4f8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -921,7 +921,7 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_); + has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 65f3772cd4d33..0a80b41c32b3b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -31,13 +31,15 @@ #include #include -#include -#include -#include +#include +#include +#include +#include #include #include #include +#include #include #include #include @@ -47,6 +49,7 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -73,6 +76,33 @@ double interpolateEgoPassMargin( } return y_vec.back(); } + +double InterpolateMap( + const std::vector & key_map, const std::vector & value_map, const double query) +{ + // If the speed is out of range of the key_map, apply zero-order hold. + if (query <= key_map.front()) { + return value_map.front(); + } + if (query >= key_map.back()) { + return value_map.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < key_map.size() - 1; ++i) { + if (key_map.at(i) <= query && query <= key_map.at(i + 1)) { + auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i)); + return interp; + } + } + + std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken." + "Please check the code." + << std::endl; + return value_map.back(); +} } // namespace class CrosswalkModule : public SceneModuleInterface @@ -113,7 +143,8 @@ class CrosswalkModule : public SceneModuleInterface double min_object_velocity; bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; - double timeout_no_intention_to_walk; + std::vector distance_map_for_no_intention_to_walk; + std::vector timeout_map_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -134,9 +165,10 @@ class CrosswalkModule : public SceneModuleInterface std::optional collision_point{}; void transitState( - const rclcpp::Time & now, const double vel, const bool is_ego_yielding, - const bool has_traffic_light, const std::optional & collision_point, - const PlannerParam & planner_param) + const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, + const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -149,8 +181,13 @@ class CrosswalkModule : public SceneModuleInterface if (!time_to_start_stopped) { time_to_start_stopped = now; } + const double distance_to_crosswalk = + bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y)); + const double timeout_no_intention_to_walk = InterpolateMap( + planner_param.distance_map_for_no_intention_to_walk, + planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; + (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { @@ -205,7 +242,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param) + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -223,7 +261,8 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, + crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; }