diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8ee3809cb19d2..3c5fab58212ed 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -147,7 +147,7 @@ - + diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 4c1ccced1bcf7..d6b79e13f1d3d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -78,7 +78,6 @@ class MultiObjectTracker : public rclcpp::Node // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::Time last_published_time_; - double publisher_period_; // internal states std::string world_frame_id_; // tracking frame diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 173cec9489f63..94062bcf9cc81 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -153,9 +153,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // If the delay compensation is enabled, the timer is used to publish the output at the correct // time. if (enable_delay_compensation) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check - const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); + const auto timer_period = rclcpp::Rate(publish_rate).period(); publish_timer_ = rclcpp::create_timer( this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); } @@ -210,35 +208,13 @@ void MultiObjectTracker::onTrigger() std::vector> objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; - onMessage(objects_list); - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - - // Publish objects if the timer is not enabled - if (publish_timer_ == nullptr) { - // if the delay compensation is disabled, publish the objects in the latest time - publish(latest_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - const rclcpp::Time publish_time = this->now(); - if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(publish_time); - } - } } void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - // Check the publish period - const auto elapsed_time = (current_time - last_published_time_).seconds(); - // If the elapsed time is over the period, publish objects with prediction - constexpr double maximum_latency_ratio = 1.11; // 11% margin - const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; - if (elapsed_time < maximum_publish_latency) return; - // get objects from the input manager and run process ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 62f9a55c75455..23f7f0b4b36c5 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -61,8 +61,10 @@ class RTCInterface bool isActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; + bool isTerminated(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); + void print() const; private: void onCooperateCommandService( diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 985e38b64f2bd..44678077d5dd1 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -20,7 +20,7 @@ namespace { using tier4_rtc_msgs::msg::Module; -std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid) { std::stringstream ss; for (auto i = 0; i < 16; ++i) { @@ -29,6 +29,34 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } +std::string command_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) { + return "ACTIVATE"; + } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) { + return "DEACTIVATE"; + } + + throw std::domain_error("invalid rtc command."); +} + +std::string state_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) { + return "WAITING_FOR_EXECUTION"; + } else if (type == tier4_rtc_msgs::msg::State::RUNNING) { + return "RUNNING"; + } else if (type == tier4_rtc_msgs::msg::State::ABORTING) { + return "ABORTING"; + } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) { + return "SUCCEEDED"; + } else if (type == tier4_rtc_msgs::msg::State::FAILED) { + return "FAILED"; + } + + throw std::domain_error("invalid rtc state."); +} + Module getModuleType(const std::string & module_name) { Module module; @@ -158,14 +186,14 @@ std::vector RTCInterface::validateCooperateCommands( } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " - << to_string(command.uuid) + << uuid_to_string(command.uuid) << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " << itr->state.type << std::endl); response.success = false; } } else { RCLCPP_WARN_STREAM( - getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) + getLogger(), "[validateCooperateCommands] uuid : " << uuid_to_string(command.uuid) << " is not found." << std::endl); response.success = false; } @@ -262,7 +290,7 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid) RCLCPP_WARN_STREAM( getLogger(), - "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); + "[removeCooperateStatus] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); } void RTCInterface::removeStoredCommand(const UUID & uuid) @@ -313,7 +341,7 @@ bool RTCInterface::isActivated(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isActivated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); return false; } @@ -338,7 +366,23 @@ bool RTCInterface::isRTCEnabled(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isRTCEnabled] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); + return is_auto_mode_enabled_; +} + +bool RTCInterface::isTerminated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + return itr->state.type == State::SUCCEEDED || itr->state.type == State::FAILED; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isTerminated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); return is_auto_mode_enabled_; } @@ -363,4 +407,16 @@ bool RTCInterface::isLocked() const return is_locked_; } +void RTCInterface::print() const +{ + RCLCPP_INFO_STREAM(getLogger(), "---print rtc cooperate statuses---" << std::endl); + for (const auto status : registered_status_.statuses) { + RCLCPP_INFO_STREAM( + getLogger(), "uuid:" << uuid_to_string(status.uuid) + << " command:" << command_to_string(status.command_status.type) + << std::boolalpha << " safe:" << status.safe + << " state:" << state_to_string(status.state.type) << std::endl); + } +} + } // namespace autoware::rtc_interface diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 02b90186d9b2f..8096d2944ee2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 7571a1f61d1c1..a7a7b30e4eec0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -114,10 +114,8 @@ void AvoidanceByLaneChange::updateSpecialData() : Direction::RIGHT; } - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, p); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, avoidance_data_, clock_.now(), planner_data_, p); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 95eb82e0b4c23..c4a05d171654d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -335,8 +335,15 @@ class SceneModuleInterface { return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { - return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - rtc.second->isActivated(uuid_map_.at(rtc.first)); + if (!rtc.second->isRegistered(uuid_map_.at(rtc.first))) { + return false; + } + + if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) { + return true; + } + + return rtc.second->isActivated(uuid_map_.at(rtc.first)); }); } @@ -345,7 +352,8 @@ class SceneModuleInterface return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - !rtc.second->isActivated(uuid_map_.at(rtc.first)); + !rtc.second->isActivated(uuid_map_.at(rtc.first)) && + !rtc.second->isTerminated(uuid_map_.at(rtc.first)); }); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index ddd0d473d2cbf..d32ab36301810 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -401,6 +401,15 @@ else (\n no) #00FFB1 :IGNORE; stop endif +if(Is UNKNOWN objects?) then (yes) +if(isSatisfiedWithUnknownTypeObjectCodition) then (yes) +#FF006C :AVOID; +stop +else (\n no) +#00FFB1 :IGNORE; +stop +endif +else (\n no) if(Is vehicle type?) then (yes) if(isSatisfiedWithVehicleCodition()) then (yes) else (\n no) @@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target start partition isObviousAvoidanceTarget() { +if(Is object within freespace?) then (yes) +if(Is object on ego lane?) then (no) +if(Is object stopping longer than threshold?) then (yes) +#FF006C :return true; +stop +else (\n no) +endif +else (\n yes) +endif +else (\n no) +endif + if(Is object within intersection?) then (yes) else (\n no) if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 3087ccc93934b..0e60e3216361d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -138,8 +138,11 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "auto" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] @@ -149,11 +152,18 @@ crosswalk: front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] # params for filtering objects that are in intersection intersection: yaw_deviation: 0.349 # [rad] (default 20.0deg) + freespace: + condition: + th_stopped_time: 5.0 # [-] + # For safety check safety_check: # safety check target type diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 49599004e0952..4e14dc4886768 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -108,7 +108,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_avoidance_for_ambiguous_vehicle{false}; + std::string policy_ambiguous_vehicle{"ignore"}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -191,8 +191,12 @@ struct AvoidanceParameters // minimum road shoulder width. maybe 0.5 [m] double object_check_min_road_shoulder_width{0.0}; + // time threshold for vehicle in freespace. + double freespace_condition_th_stopped_time{0.0}; + // force avoidance - double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; + double wait_and_see_th_closest_distance{0.0}; double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 316501fbbd37f..b205838896038 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -47,6 +47,11 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray( const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy); + +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); + MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 8c46affbc64e3..bfeb942c82be3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -325,10 +326,27 @@ class AvoidanceHelper const auto object = objects.front(); + // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running + // as AUTO mode. if (!object.is_ambiguous) { return true; } + // check only front objects. + if (object.longitudinal < 0.0) { + return true; + } + + // if the policy is "manual", this module generates candidate path and waits approval. + if (parameters_->policy_ambiguous_vehicle == "manual") { + return false; + } + + // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle. + if (!isWaitAndSeeTarget(object)) { + return true; + } + if (!object.avoid_margin.has_value()) { return true; } @@ -341,9 +359,32 @@ class AvoidanceHelper const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < - prepare_distance + constant_distance + avoidance_distance + - parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + + parameters_->wait_and_see_th_closest_distance; + } + + bool isWaitAndSeeTarget(const ObjectData & object) const + { + const auto & behaviors = parameters_->wait_and_see_target_behaviors; + if (object.behavior == ObjectData::Behavior::MERGING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "MERGING"; + }); + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "DEVIATING"; + }); + } + + if (object.behavior == ObjectData::Behavior::NONE) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "NONE"; + }); + } + + return false; } static bool isAbsolutelyNotAvoidable(const ObjectData & object) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 84cf7c4e33d26..56ac3d7f4f1bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = @@ -152,6 +154,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + p.freespace_condition_th_stopped_time = + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 635edb7c84f40..34d06a46d9ac8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface */ void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; + /** + * @brief fill additional data which are necessary to plan avoidance path/velocity. + * @param avoidance target objects. + */ + void fillAvoidanceTargetData(ObjectDataArray & objects) const; + /** * @brief fill candidate shift lines. * @param avoidance data. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index 1f68ef7d49c47..909d28ed4bab6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -117,15 +117,12 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, - const std::shared_ptr & parameters); - void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data); -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects); +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index db3215fa8d238..38a91ac39525b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -716,9 +716,10 @@ "avoidance_for_ambiguous_vehicle": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "Enable avoidance maneuver for ambiguous vehicles.", + "policy": { + "type": "string", + "enum": ["auto", "manual", "ignore"], + "description": "Ego behavior policy for ambiguous vehicle.", "default": "true" }, "closest_distance_to_wait_and_see": { @@ -778,14 +779,26 @@ }, "required": ["traffic_light", "crosswalk"], "additionalProperties": false + }, + "wait_and_see": { + "type": "object", + "properties": { + "target_behaviors": { + "type": "array", + "default": ["MERGING", "DEVIATING"], + "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold." + }, + "th_closest_distance": { + "type": "number", + "default": 10.0, + "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle." + } + }, + "required": ["target_behaviors", "th_closest_distance"], + "additionalProperties": false } }, - "required": [ - "enable", - "closest_distance_to_wait_and_see", - "condition", - "ignore_area" - ], + "required": ["policy", "condition", "ignore_area", "wait_and_see"], "additionalProperties": false }, "intersection": { @@ -799,6 +812,25 @@ }, "required": ["yaw_deviation"], "additionalProperties": false + }, + "freespace": { + "type": "object", + "properties": { + "condition": { + "type": "object", + "properties": { + "th_stopped_time": { + "type": "number", + "description": "This module delays avoidance maneuver to see vehicle behavior in freespace.", + "default": 5.0 + } + }, + "required": ["th_stopped_time"], + "additionalProperties": false + } + }, + "required": ["condition"], + "additionalProperties": false } }, "required": [ @@ -810,7 +842,8 @@ "merging_vehicle", "parked_vehicle", "avoidance_for_ambiguous_vehicle", - "intersection" + "intersection", + "freespace" ], "additionalProperties": false }, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index e9838eb4a2cfc..a542107e890aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -456,7 +456,7 @@ MarkerArray createOtherObjectsMarkerArray( appendMarkerArray( createObjectsCubeMarkerArray( filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.0, 1.0, 0.0, 0.8)), + createMarkerColor(0.5, 0.5, 0.5, 0.8)), &msg); appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); @@ -466,6 +466,94 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy) +{ + MarkerArray msg; + + if (policy != "manual") { + return msg; + } + + for (const auto & object : objects) { + if (!object.is_ambiguous || !object.is_avoidable) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW, + createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + Point src, dst; + src = object.getPosition(); + src.z += 4.0; + dst = object.getPosition(); + dst.z += 2.0; + + marker.points.push_back(src); + marker.points.push_back(dst); + marker.id = uuidToInt32(object.object.object_id); + + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L, + Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), + createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.getPose(); + marker.pose.position.z += 4.5; + std::ostringstream string_stream; + string_stream << "SHOULD AVOID?"; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = ego_pose; + marker.pose.position.z += 2.0; + std::ostringstream string_stream; + string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT."; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + return msg; + } + + return msg; +} + +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data) +{ + MarkerArray msg; + + if (!data.stop_target_object.has_value()) { + return msg; + } + + appendMarkerArray( + createObjectsCubeMarkerArray( + {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9), + createMarkerColor(1.0, 0.0, 0.42, 0.5)), + &msg); + + return msg; +} + MarkerArray createDrivableBounds( const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, const float & b) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 2d02f33e19870..5655696ff086d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; - updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); updateParam( - parameters, ns + "closest_distance_to_wait_and_see", - p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); updateParam( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( @@ -151,6 +150,12 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( p->object_ignore_section_crosswalk_behind_distance); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + updateParam( + parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time); + } + { const std::string ns = "avoidance.target_filtering.intersection."; updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b7ab41ec99abd..d125f043f477b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -288,16 +288,22 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - // target objects for avoidance + // filter only for the latest detected objects. fillAvoidanceTargetObjects(data, debug); - // lost object compensation - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, data.target_objects, parameters_); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, data.target_objects, data.other_objects); + // compensate lost object which was avoidance target. if the time hasn't passed more than + // threshold since perception module lost the target yet, this module keeps it as avoidance + // target. + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, data, clock_->now(), planner_data_, parameters_); + + // once an object filtered for boundary clipping, this module keeps the information until the end + // of execution. utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data); + // calculate various data for each target objects. + fillAvoidanceTargetData(data.target_objects); + // sort object order by longitudinal distance std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { return a.longitudinal < b.longitudinal; @@ -310,8 +316,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -347,15 +352,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); updateRoadShoulderDistance(data, planner_data_, parameters_); - // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { - fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); - o.to_stop_line = calcDistanceToStopLine(o); - fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); - }); - // debug { std::vector debug_info_array; @@ -373,6 +369,22 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } +void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + + // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); + std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); + o.to_stop_line = calcDistanceToStopLine(o); + fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); + }); +} + ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { @@ -1392,11 +1404,19 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; + using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); + appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); + appendMarkerArray( + createAmbiguousObjectsMarkerArray( + data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle), + &info_marker_); } void StaticObstacleAvoidanceModule::updateDebugMarker( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index c3a67eb074d73..5e58466fa4229 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -239,10 +239,18 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; + const auto is_on_path = [this](const auto & object) { + const auto [overhang, point] = object.overhang_points.front(); + return std::abs(overhang) < 0.5 * data_->parameters.vehicle_width; + }; + const auto is_valid_shift_line = [](const auto & s) { return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; + ObjectDataArray unavoidable_objects; + + // target objects are sorted by longitudinal distance. AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { @@ -253,22 +261,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; } - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } - const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o); const auto desire_shift_length = - helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); - if (utils::static_obstacle_avoidance::isSameDirectionShift( - is_object_on_right, desire_shift_length)) { + helper_->getShiftLength(o, isOnRight(o), o.avoid_margin.value()); + if (utils::static_obstacle_avoidance::isSameDirectionShift(isOnRight(o), desire_shift_length)) { o.info = ObjectInfo::SAME_DIRECTION_SHIFT; - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } @@ -276,13 +284,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); if (!feasible_shift_profile.has_value()) { - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } + // If there is an object that cannot be avoided, this module only avoids object on the same side + // as unavoidable object. + if (!unavoidable_objects.empty()) { + if (isOnRight(unavoidable_objects.front()) && !isOnRight(o)) { + break; + } + if (!isOnRight(unavoidable_objects.front()) && isOnRight(o)) { + break; + } + } + // use absolute dist for return-to-center, relative dist from current for avoiding. const auto feasible_return_distance = helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 016972dccb38e..0830c0f10dd4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -339,6 +339,32 @@ bool isWithinIntersection( lanelet::utils::to2D(polygon.basicPolygon())); } +bool isWithinFreespace( + const ObjectData & object, const std::shared_ptr & route_handler) +{ + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return false; + } + + std::sort(polygons.begin(), polygons.end(), [&object](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + lanelet::utils::to2D(a).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + const double b_distance = boost::geometry::distance( + lanelet::utils::to2D(b).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + return a_distance < b_distance; + }); + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygons.front().basicPolygon())); +} + bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { if (boost::geometry::within( @@ -708,6 +734,14 @@ bool isObviousAvoidanceTarget( [[maybe_unused]] const std::shared_ptr & planner_data, [[maybe_unused]] const std::shared_ptr & parameters) { + if (isWithinFreespace(object, planner_data->route_handler)) { + if (!object.is_on_ego_lane) { + if (object.stop_time > parameters->freespace_condition_th_stopped_time) { + return true; + } + } + } + if (!object.is_within_intersection) { if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle."); @@ -880,7 +914,7 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + if (parameters->policy_ambiguous_vehicle == "ignore") { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -889,6 +923,7 @@ bool isSatisfiedWithVehicleCondition( object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -897,6 +932,7 @@ bool isSatisfiedWithVehicleCondition( parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -907,22 +943,9 @@ bool isSatisfiedWithVehicleCondition( return true; } } else { - if (object.behavior == ObjectData::Behavior::MERGING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::NONE) { - object.is_ambiguous = false; - return true; - } + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = true; + return true; } object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; @@ -1645,31 +1668,39 @@ void fillObjectStoppableJudge( object_data.is_stoppable = same_id_obj->is_stoppable; } -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto updateIfDetectedNow = [&now_objects](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + const auto include = [](const auto & objects, const auto & search_id) { + return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) { + return o.object.object_id != search_id; + }); + }; + + // STEP.1 UPDATE STORED OBJECTS. + const auto match = [&data](auto & object) { + const auto & search_id = object.object.object_id; + const auto same_id_object = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [&search_id](const auto & o) { return o.object.object_id == search_id; }); // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; + if (same_id_object != data.target_objects.end()) { + object = *same_id_object; return true; } - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.getPose(); - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.getPose()) < POS_THR; - }); + const auto similar_pos_obj = std::find_if( + data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) { + constexpr auto POS_THR = 1.5; + return calcDistance2d(object.getPose(), o.getPose()) < POS_THR; + }); // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; + if (similar_pos_obj != data.target_objects.end()) { + object = *similar_pos_obj; return true; } @@ -1677,61 +1708,54 @@ void updateRegisteredObject( return false; }; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); - - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects.size()) - 1; i >= 0; --i) { - auto & r = registered_objects.at(i); - - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (now - r.last_seen).seconds(); - } else { - r.last_seen = now; - r.lost_time = 0.0; - } + // STEP1-1: REMOVE EXPIRED OBJECTS. + const auto itr = std::remove_if( + stored_objects.begin(), stored_objects.end(), [&now, &match, ¶meters](auto & o) { + if (!match(o)) { + o.lost_time = (now - o.last_seen).seconds(); + } else { + o.last_seen = now; + o.lost_time = 0.0; + } - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters->object_last_seen_threshold) { - registered_objects.erase(registered_objects.begin() + i); - } - } + return o.lost_time > parameters->object_last_seen_threshold; + }); - const auto isAlreadyRegistered = [&](const auto & n_id) { - const auto & r = registered_objects; - return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); - }; + stored_objects.erase(itr, stored_objects.end()); - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects.push_back(now_obj); + // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS. + for (const auto & current_object : data.target_objects) { + if (!include(stored_objects, current_object.object.object_id)) { + stored_objects.push_back(current_object); } } -} -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects) -{ - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; + // STEP2: COMPENSATE CURRENT TARGET OBJECTS + const auto is_detected = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.target_objects.begin(), data.target_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; + const auto is_ignored = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.other_objects.begin(), data.other_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - for (const auto & registered : registered_objects) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); + for (auto & stored_object : stored_objects) { + if (is_detected(stored_object.object.object_id)) { + continue; } + if (is_ignored(stored_object.object.object_id)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, ego_pos, stored_object); + + data.target_objects.push_back(stored_object); } } @@ -1852,17 +1876,19 @@ void filterTargetObjects( utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - // TODO(Satoshi Ota) parametrize stop time threshold if need. - constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (filtering_utils::isUnknownTypeObject(o)) { + // TARGET: UNKNOWN + + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (o.stop_time < STOP_TIME_THRESHOLD) { o.info = ObjectInfo::UNSTABLE_OBJECT; data.other_objects.push_back(o); continue; } - } + } else if (filtering_utils::isVehicleTypeObject(o)) { + // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE - if (filtering_utils::isVehicleTypeObject(o)) { o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = @@ -1879,6 +1905,8 @@ void filterTargetObjects( continue; } } else { + // TARGET: PEDESTRIAN, BICYCLE + o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index e57b5a45d8be6..a0cf69ee71027 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -19,6 +19,7 @@ predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -27,7 +28,8 @@ action: # action to insert in the trajectory if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory - distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle + lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 0c793c9f5798e..8b03fa66eab55 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -70,9 +70,11 @@ std::optional calculate_last_in_lane_pose( std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params) + const std::optional & prev_slowdown_point, PlannerParam params) { - params.extra_front_offset += params.dist_buffer; + params.extra_front_offset += params.lon_dist_buffer; + params.extra_right_offset += params.lat_dist_buffer; + params.extra_left_offset += params.lat_dist_buffer; const auto base_footprint = make_base_footprint(params); // search for the first slowdown decision for which a stop point can be inserted diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index ee4897de86c5b..145f21c94c0d0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -53,6 +53,6 @@ std::optional calculate_last_in_lane_pose( /// @return optional slowdown point to insert in the trajectory std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params); + const std::optional & prev_slowdown_point, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index ecc10df43c792..1231bf49759d5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -111,6 +111,14 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( }) != object.classification.end(); if (is_pedestrian) continue; + const auto is_coming_from_behind = + motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, + object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + if (params.objects_ignore_behind_ego && is_coming_from_behind) { + continue; + } + auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 8397d0c116090..0afedcd7f9c9a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include namespace autoware::motion_velocity_planner @@ -85,6 +84,8 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); pp.objects_cut_predicted_paths_beyond_red_lights = getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + pp.objects_ignore_behind_ego = + getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); @@ -93,7 +94,9 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); - pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.lon_dist_buffer = + getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); + pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); pp.slow_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); @@ -132,14 +135,15 @@ void OutOfLaneModule::update_parameters(const std::vector & p updateParam( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); - + updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); - updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); + updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 7a71bb6a7a28a..9c68a0bf23a92 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -56,12 +56,14 @@ struct PlannerParam double objects_min_confidence; // minimum confidence to consider a predicted path double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in // the other lane + bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the trajectory if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double dist_buffer; + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index a8b84fa3dafda..9be416f9e9bb0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -418,7 +418,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); - set_velocity_smoother_params(); + // set_velocity_smoother_params(); TODO(Maxime): fix update parameters of the velocity smoother rcl_interfaces::msg::SetParametersResult result; result.successful = true;