From e48f4d604b6e799aae0dd45bb959a1350e08b474 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 5 Feb 2024 18:28:17 +0900 Subject: [PATCH 1/8] pedestrians' intention estimation feature against the green signal Signed-off-by: Yuki Takagi --- .../map_based_prediction_node.hpp | 10 +- .../src/map_based_prediction_node.cpp | 96 +++++++++++++++++-- 2 files changed, 96 insertions(+), 10 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 2864d1c5bf393..22f66ff9a3848 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -131,6 +131,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Object History std::unordered_map> objects_history_; + std::map, rclcpp::Time> stopped_times_against_green_; // Lanelet Map Pointers std::shared_ptr lanelet_map_ptr_; @@ -190,6 +191,9 @@ class MapBasedPredictionNode : public rclcpp::Node double acceleration_exponential_half_life_; bool use_crosswalk_signal_; + double threshold_speed_as_stopping_; + std::vector distance_map_for_no_intention_to_walk_; + std::vector timeout_map_for_no_intention_to_walk_; // Stop watch StopWatch stop_watch_; @@ -214,7 +218,8 @@ class MapBasedPredictionNode : public rclcpp::Node PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object); - void removeOldObjectsHistory(const double current_time); + void removeOldObjectsHistory( + const double current_time, const TrackedObjects::ConstSharedPtr in_objects); LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( @@ -262,6 +267,9 @@ class MapBasedPredictionNode : public rclcpp::Node const PredictedPath & predicted_path, const std::vector & predicted_paths); std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); std::optional getTrafficSignalElement(const lanelet::Id & id); + bool calcIntentionToCrossWithTrafficSgnal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id); visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 168b874ccbcce..5b85a7e4d1edb 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -20,6 +20,7 @@ #include #include #include +// #include #include #include #include @@ -788,7 +789,13 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ acceleration_exponential_half_life_ = declare_parameter("acceleration_exponential_half_life"); - use_crosswalk_signal_ = declare_parameter("use_crosswalk_signal"); + use_crosswalk_signal_ = declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); + threshold_speed_as_stopping_ = + declare_parameter("crosswalk_with_signal.threshold_speed_as_stopping"); + distance_map_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.distance_map_for_no_intention_to_walk"); + timeout_map_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.timeout_map_for_no_intention_to_walk"); path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, @@ -912,7 +919,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Remove old objects information in object history const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); - removeOldObjectsHistory(objects_detected_time); + removeOldObjectsHistory(objects_detected_time, in_objects); // result output PredictedObjects output; @@ -1229,16 +1236,14 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { - const auto signal_color = [&] { - const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value()); - return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; - }(); - - if (signal_color == TrafficSignalElement::RED) { + if (!calcIntentionToCrossWithTrafficSgnal( + object, crosswalk, crosswalk_signal_id_opt.value())) { continue; } } @@ -1330,7 +1335,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) return; } -void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) +void MapBasedPredictionNode::removeOldObjectsHistory( + const double current_time, const TrackedObjects::ConstSharedPtr in_objects) { std::vector invalid_object_id; for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) { @@ -1371,6 +1377,19 @@ void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) for (const auto & key : invalid_object_id) { objects_history_.erase(key); } + + for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { + const bool isDisappeared = std::none_of( + in_objects->objects.begin(), in_objects->objects.end(), + [&it](autoware_auto_perception_msgs::msg::TrackedObject obj) { + return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first; + }); + if (isDisappeared) { + it = stopped_times_against_green_.erase(it); + } else { + ++it; + } + } } LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) @@ -2268,6 +2287,65 @@ std::optional MapBasedPredictionNode::getTrafficSignalElem return std::nullopt; } +bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSgnal( + const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, + const lanelet::Id & signal_id) +{ + const auto signal_color = [&] { + const auto elem_opt = getTrafficSignalElement(signal_id); + return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; + }(); + + const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id); + if ( + signal_color == TrafficSignalElement::GREEN && + tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < + threshold_speed_as_stopping_) { + stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); + + const auto obj_position = object.kinematics.pose_with_covariance.pose.position; + const double distance_to_crosswalk = boost::geometry::distance( + crosswalk.polygon2d().basicPolygon(), lanelet::BasicPoint2d(obj_position.x, obj_position.y)); + + auto InterpolateMap = []( + const std::vector & key_set, + const std::vector & value_set, const double query) { + if (query <= key_set.front()) { + return value_set.front(); + } else if (query >= key_set.back()) { + return value_set.back(); + } + for (size_t i = 0; i < key_set.size() - 1; ++i) { + if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { + auto ratio = + (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); + } + } + return value_set.back(); + }; + + const double timeout_no_intention_to_walk = InterpolateMap( + distance_map_for_no_intention_to_walk_, timeout_map_for_no_intention_to_walk_, + distance_to_crosswalk); + if ( + (this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() > + timeout_no_intention_to_walk) { + return false; + } + } else { + stopped_times_against_green_.erase(key); + // If the pedestrian disappears, another function erases the old data. + } + + if (signal_color == TrafficSignalElement::RED) { + return false; + } + + return true; +} + } // namespace map_based_prediction #include From baa28399b29fd269ff7fe1fc1a4621b058b39210 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 11:49:17 +0900 Subject: [PATCH 2/8] delete the reimplimented feature Signed-off-by: Yuki Takagi --- .../src/manager.cpp | 2 -- .../src/scene_crosswalk.hpp | 17 +++++------------ 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 094369b0a7a3c..6125bf39e7e98 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -101,8 +101,6 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); - cp.disable_stop_for_yield_cancel = - 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.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ed1e342df9f7a..0b9d066ad5546 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -141,7 +141,6 @@ class CrosswalkModule : public SceneModuleInterface double min_jerk_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; - bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; std::vector distance_map_for_no_intention_to_walk; std::vector timeout_map_for_no_intention_to_walk; @@ -168,9 +167,8 @@ class CrosswalkModule : public SceneModuleInterface void transitState( 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_ego_yielding, const std::optional & collision_point, + const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -190,9 +188,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (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) { + if (is_ego_yielding && !intent_to_cross) { collision_state = CollisionState::IGNORE; return; } @@ -252,9 +248,7 @@ class CrosswalkModule : public SceneModuleInterface // add new object if (objects.count(uuid) == 0) { - if ( - has_traffic_light && planner_param.disable_stop_for_yield_cancel && - planner_param.disable_yield_for_new_stopped_object) { + if (has_traffic_light && planner_param.disable_yield_for_new_stopped_object) { objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); @@ -263,8 +257,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, - crosswalk_polygon); + now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification; From 8bc48e44757c134332a8337fa1df2e06c12e65fa Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 14:21:30 +0900 Subject: [PATCH 3/8] sync params Signed-off-by: Yuki Takagi --- .../config/map_based_prediction.param.yaml | 7 ++++++- .../config/crosswalk.param.yaml | 1 - 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index ed1193f1c49a0..adb8d37a4a175 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -19,7 +19,12 @@ use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds - use_crosswalk_signal: true + crosswalk_with_signal: + use_crosswalk_signal: true + threshold_speed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped + # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map + timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 29079b99b6718..4abdbfbee419e 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -53,7 +53,6 @@ stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order From 6f5be123fb99ea87648c9f221da9a472c7146f6d Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 15:47:47 +0900 Subject: [PATCH 4/8] refactor Signed-off-by: Yuki Takagi --- .../config/map_based_prediction.param.yaml | 4 +- .../map_based_prediction_node.hpp | 5 +- .../src/map_based_prediction_node.cpp | 62 ++++++++++--------- .../README.md | 6 +- .../config/crosswalk.param.yaml | 4 +- .../src/manager.cpp | 8 +-- .../src/scene_crosswalk.hpp | 8 +-- 7 files changed, 50 insertions(+), 47 deletions(-) diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index adb8d37a4a175..67c158901cd14 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -23,8 +23,8 @@ use_crosswalk_signal: true threshold_speed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. - distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map - timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map + distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map + timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 22f66ff9a3848..2ebc50fe992f7 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -192,8 +193,8 @@ class MapBasedPredictionNode : public rclcpp::Node bool use_crosswalk_signal_; double threshold_speed_as_stopping_; - std::vector distance_map_for_no_intention_to_walk_; - std::vector timeout_map_for_no_intention_to_walk_; + std::vector distance_set_for_no_intention_to_walk_; + std::vector timeout_set_for_no_intention_to_walk_; // Stop watch StopWatch stop_watch_; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 5b85a7e4d1edb..987380a60f9db 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -20,7 +20,6 @@ #include #include #include -// #include #include #include #include @@ -792,10 +791,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ use_crosswalk_signal_ = declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); threshold_speed_as_stopping_ = declare_parameter("crosswalk_with_signal.threshold_speed_as_stopping"); - distance_map_for_no_intention_to_walk_ = declare_parameter>( - "crosswalk_with_signal.distance_map_for_no_intention_to_walk"); - timeout_map_for_no_intention_to_walk_ = declare_parameter>( - "crosswalk_with_signal.timeout_map_for_no_intention_to_walk"); + distance_set_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.distance_set_for_no_intention_to_walk"); + timeout_set_for_no_intention_to_walk_ = declare_parameter>( + "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, @@ -1238,7 +1237,6 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } // try to find the edge points for all crosswalks and generate path to the crosswalk edge - for (const auto & crosswalk : crosswalks_) { const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { @@ -2303,37 +2301,41 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSgnal( threshold_speed_as_stopping_) { stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); - const auto obj_position = object.kinematics.pose_with_covariance.pose.position; - const double distance_to_crosswalk = boost::geometry::distance( - crosswalk.polygon2d().basicPolygon(), lanelet::BasicPoint2d(obj_position.x, obj_position.y)); - - auto InterpolateMap = []( - const std::vector & key_set, - const std::vector & value_set, const double query) { - if (query <= key_set.front()) { - return value_set.front(); - } else if (query >= key_set.back()) { - return value_set.back(); - } - for (size_t i = 0; i < key_set.size() - 1; ++i) { - if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { - auto ratio = - (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); - ratio = std::clamp(ratio, 0.0, 1.0); - return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); + const auto timeout_no_intention_to_walk = [&]() { + auto InterpolateMap = []( + const std::vector & key_set, + const std::vector & value_set, const double query) { + if (query <= key_set.front()) { + return value_set.front(); + } else if (query >= key_set.back()) { + return value_set.back(); } - } - return value_set.back(); - }; + for (size_t i = 0; i < key_set.size() - 1; ++i) { + if (key_set.at(i) <= query && query <= key_set.at(i + 1)) { + auto ratio = + (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i)); + } + } + return value_set.back(); + }; + + const auto obj_position = object.kinematics.pose_with_covariance.pose.position; + const double distance_to_crosswalk = boost::geometry::distance( + crosswalk.polygon2d().basicPolygon(), + lanelet::BasicPoint2d(obj_position.x, obj_position.y)); + return InterpolateMap( + distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_, + distance_to_crosswalk); + }(); - const double timeout_no_intention_to_walk = InterpolateMap( - distance_map_for_no_intention_to_walk_, timeout_map_for_no_intention_to_walk_, - distance_to_crosswalk); if ( (this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() > timeout_no_intention_to_walk) { return false; } + } else { stopped_times_against_green_.erase(key); // If the pedestrian disappears, another function erases the old data. diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index c1ddc28e03426..24adac131be42 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -118,14 +118,14 @@ To prevent such a deadlock situation, the ego will cancel yielding depending on For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. \*1: -The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. +The time is calculated by the interpolation of distance between the object and crosswalk with `distance_set_for_no_intention_to_walk` and `timeout_set_for_no_intention_to_walk`. In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | | --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | -| `distance_map_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | -| `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | +| `distance_set_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | +| `timeout_set_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | \*2: In the `pass_judge` namespace, the following parameters are defined. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 4abdbfbee419e..73d92a8f41234 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -55,8 +55,8 @@ ## param for yielding disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. - distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # 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 6125bf39e7e98..1fa9247548add 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -103,10 +103,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); cp.disable_yield_for_new_stopped_object = getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - 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.distance_set_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_set_for_no_intention_to_walk"); + cp.timeout_set_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_set_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.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 0b9d066ad5546..d1196bb7b4a9a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -142,8 +142,8 @@ class CrosswalkModule : public SceneModuleInterface double stop_object_velocity; double min_object_velocity; bool disable_yield_for_new_stopped_object; - std::vector distance_map_for_no_intention_to_walk; - std::vector timeout_map_for_no_intention_to_walk; + std::vector distance_set_for_no_intention_to_walk; + std::vector timeout_set_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -184,8 +184,8 @@ class CrosswalkModule : public SceneModuleInterface 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); + planner_param.distance_set_for_no_intention_to_walk, + planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if (is_ego_yielding && !intent_to_cross) { From 12030135a605a66c40c4afd2d5cb6e867b052922 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 19:09:58 +0900 Subject: [PATCH 5/8] update docs Signed-off-by: Yuki Takagi --- perception/map_based_prediction/README.md | 26 +++++-- .../README.md | 72 +++++++++---------- 2 files changed, 54 insertions(+), 44 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 2419cdb010799..066708ed8510a 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -191,6 +191,11 @@ This module treats **Pedestrians** and **Bicycles** as objects using the crosswa If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point. +This module takes into account the corresponding traffic light information. +When RED signal is indicated, we assume the target object will not walk across. +As an optional feature, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either. +This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross. +
@@ -201,14 +206,27 @@ If the target object is inside the road or crosswalk, this module outputs one or +\*1: +The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`. + +
+ + + + + +
+
+ ## Inputs / Outputs ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- | -| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | -| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | +| Name | Type | Description | +| -------------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------------------------- | +| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. | +| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. | +| '~/perception/traffic_light_recognition/traffic_signals' | 'autoware_perception_msgs::msg::TrafficSignalArray;' | rearranged information on the corresponding traffic lights | ### Output diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 24adac131be42..10de7e9955663 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -10,7 +10,7 @@ This module judges whether the ego should stop in front of the crosswalk in orde ## Features -### Yield +### Yield the Way to the Pedestrians #### Target Object @@ -97,6 +97,17 @@ In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `e In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. +Of course, if the red signal is indicating to the corresponding croswalk, the ego do not yield against the pedestians. + +
+ + + + + +
+
+ In the `pass_judge` namespace, the following parameters are defined. | Parameter | | Type | Description | @@ -108,13 +119,11 @@ In the `pass_judge` namespace, the following parameters are defined. | `ego_pass_later_margin_y` | [[s]] | double | time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | | `ego_pass_later_additional_margin` | [s] | double | additional time margin for object pass first situation to suppress chattering | -### Smooth Yield Decision +#### Smooth Yield Decision If the object is stopped near the crosswalk but has no intention of walking, a situation can arise in which the ego continues to yield the right-of-way to the object. To prevent such a deadlock situation, the ego will cancel yielding depending on the situation. -#### Cases without traffic lights - For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. \*1: @@ -122,10 +131,10 @@ The time is calculated by the interpolation of distance between the object and c In the `pass_judge` namespace, the following parameters are defined. -| Parameter | | Type | Description | -| --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | -| `distance_set_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | -| `timeout_set_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | +| Parameter | | Type | Description | +| --------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------- | +| `distance_set_for_no_intention_to_walk` | [[m]] | double | key sets to calculate the timeout for no intention to walk with interpolation | +| `timeout_set_for_no_intention_to_walk` | [[s]] | double | value sets to calculate the timeout for no intention to walk with interpolation | \*2: In the `pass_judge` namespace, the following parameters are defined. @@ -134,23 +143,6 @@ In the `pass_judge` namespace, the following parameters are defined. | ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | | `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -#### Cases with traffic lights - -The ego will cancel the yield without stopping when the object stops around the crosswalk but has no intention to walk (\*1). -This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. - -\*1: -The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`. - -
- - - - - -
-
- #### New Object Handling Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. @@ -165,21 +157,7 @@ In the `pass_judge` namespace, the following parameters are defined. | -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | | `disable_yield_for_new_stopped_object` | [-] | bool | If set to true, the new stopped object will be ignored around the crosswalk with a traffic light | -### Safety Slow Down Behavior - -In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. -However, it may be desirable to slow down in situations, for example, where there are blind spots. -Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) -document. - -| Parameter | | Type | Description | -| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | -| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | -| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | -| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | -| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | - -### Stuck Vehicle Detection +### Stuck Prevention on the Crosswalk The feature will make the ego not to stop on the crosswalk. When there is a low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module plans to stop before the crosswalk even if there are no pedestrians or bicycles. @@ -201,6 +179,20 @@ In the `stuck_vehicle` namespace, the following parameters are defined. | `min_jerk` | [m/sss] | double | minimum jerk to stop | | `max_jerk` | [m/sss] | double | maximum jerk to stop | +### Safety Slow Down Behavior + +In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. +However, it may be desirable to slow down in situations, for example, where there are blind spots. +Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +document. + +| Parameter | | Type | Description | +| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | +| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | +| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | +| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | +| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | + ### Others In the `common` namespace, the following parameters are defined. From 880762d81d1c3a7e1b07427fe9a91c19d23ef284 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 19:18:29 +0900 Subject: [PATCH 6/8] update docs Signed-off-by: Yuki Takagi --- perception/map_based_prediction/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 066708ed8510a..d95a32b475bcf 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -193,7 +193,7 @@ If there are a reachable crosswalk entry points within the `prediction_time_hori This module takes into account the corresponding traffic light information. When RED signal is indicated, we assume the target object will not walk across. -As an optional feature, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either. +In additon, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either. This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross.
From 6f170cec2340daab836f44a73dcf7dc4c864cc6e Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 6 Feb 2024 19:40:31 +0900 Subject: [PATCH 7/8] update docs Signed-off-by: Yuki Takagi --- .../map_based_prediction/config/map_based_prediction.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 67c158901cd14..7035788d6540a 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -22,7 +22,7 @@ crosswalk_with_signal: use_crosswalk_signal: true threshold_speed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped - # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. + # If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk. distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map # parameter for shoulder lane prediction From d82c6d5e68f2e1833c67797736e23bb382fc49e0 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 7 Feb 2024 14:45:11 +0900 Subject: [PATCH 8/8] refactor Signed-off-by: Yuki Takagi --- perception/map_based_prediction/README.md | 12 ------------ .../config/map_based_prediction.param.yaml | 2 +- .../map_based_prediction_node.hpp | 2 +- .../src/map_based_prediction_node.cpp | 6 +++--- .../behavior_velocity_crosswalk_module/README.md | 2 +- 5 files changed, 6 insertions(+), 18 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index d95a32b475bcf..bb70db1bb0fbf 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -206,18 +206,6 @@ If the target object is inside the road or crosswalk, this module outputs one or
-\*1: -The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`. - -
- - - - - -
-
- ## Inputs / Outputs ### Input diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 7035788d6540a..f8ad371ab92a6 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -21,7 +21,7 @@ acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds crosswalk_with_signal: use_crosswalk_signal: true - threshold_speed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped + threshold_velocity_assumed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped # If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk. distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 2ebc50fe992f7..954d0dde0f2bb 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -192,7 +192,7 @@ class MapBasedPredictionNode : public rclcpp::Node double acceleration_exponential_half_life_; bool use_crosswalk_signal_; - double threshold_speed_as_stopping_; + double threshold_velocity_assumed_as_stopping_; std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 987380a60f9db..9cdd02427700c 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -789,8 +789,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("acceleration_exponential_half_life"); use_crosswalk_signal_ = declare_parameter("crosswalk_with_signal.use_crosswalk_signal"); - threshold_speed_as_stopping_ = - declare_parameter("crosswalk_with_signal.threshold_speed_as_stopping"); + threshold_velocity_assumed_as_stopping_ = + declare_parameter("crosswalk_with_signal.threshold_velocity_assumed_as_stopping"); distance_set_for_no_intention_to_walk_ = declare_parameter>( "crosswalk_with_signal.distance_set_for_no_intention_to_walk"); timeout_set_for_no_intention_to_walk_ = declare_parameter>( @@ -2298,7 +2298,7 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSgnal( if ( signal_color == TrafficSignalElement::GREEN && tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) < - threshold_speed_as_stopping_) { + threshold_velocity_assumed_as_stopping_) { stopped_times_against_green_.try_emplace(key, this->get_clock()->now()); const auto timeout_no_intention_to_walk = [&]() { diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 10de7e9955663..4c7b214763c31 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -97,7 +97,7 @@ In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `e In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. -Of course, if the red signal is indicating to the corresponding croswalk, the ego do not yield against the pedestians. +If the red signal is indicating to the corresponding crosswalk, the ego do not yield against the pedestrians.