From 4db9fc5231c530d8ad15ea46050f3adc7fdfaade Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Nov 2023 03:31:22 +0900 Subject: [PATCH 01/18] fix(mission_planner): revert "refactor(mission_planner): use combineLaneletsShape in lanele2_extension (#5535)" (#5543) Revert "refactor(mission_planner): use combineLaneletsShape in lanelet2_extension (#5535)" This reverts commit c4ca645cbbd0a7b919ba1c2cdf78742bb81b87b5. --- .../src/lanelet2_plugins/default_planner.cpp | 4 +-- .../lanelet2_plugins/utility_functions.cpp | 33 +++++++++++++++++++ .../lanelet2_plugins/utility_functions.hpp | 1 + 3 files changed, 36 insertions(+), 2 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 0fce63827b5e7..673519b6f7a0e 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -288,7 +288,7 @@ bool DefaultPlanner::check_goal_footprint( lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = lanelet::utils::combineLaneletsShape(lanelets); + lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); // if next lanelet length longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -347,7 +347,7 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets - lanelet::ConstLanelet combined_prev_lanelet = lanelet::utils::combineLaneletsShape(path_lanelets); + lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 126c673108b3c..690a864fcdacd 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -54,6 +54,39 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + std::vector left_bound_ids; + std::vector right_bound_ids; + + for (const auto & llt : lanelets) { + if (llt.id() != 0) { + left_bound_ids.push_back(llt.leftBound().id()); + right_bound_ids.push_back(llt.rightBound().id()); + } + } + + for (const auto & llt : lanelets) { + if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { + for (const auto & pt : llt.leftBound()) { + lefts.push_back(lanelet::Point3d(pt)); + } + } + if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return std::move(combined_lanelet); +} + std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet) { std::vector centerline_points; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index df9d42bab9444..3ea6237f38501 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -49,6 +49,7 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); From 32c897d54dc4fd84e4d5e401334d587b19c1318c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Nov 2023 13:50:16 +0900 Subject: [PATCH 02/18] fix(tier4_planning_launch): obstacle_cruise_planner pipeline is not connected (#5542) Signed-off-by: Takayuki Murooka --- .../lane_driving/motion_planning/motion_planning.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index a8dbcfd372226..18de04fd9e317 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -127,8 +127,8 @@ - - + + From ce0f11ab2f688f69a89c178b64c34f68de44f985 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 10 Nov 2023 15:19:46 +0900 Subject: [PATCH 03/18] fix(image_projection_based_fusion): add missing install (#5548) --- perception/image_projection_based_fusion/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 53aafa44b75d6..c021dd92dae64 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -146,6 +146,10 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE pointpainting_fusion_node ) + install( + TARGETS pointpainting_cuda_lib + DESTINATION lib + ) else() message("Skipping build of some nodes due to missing dependencies") endif() From 9000f430c937764c14e43109539302f1f878ed70 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 10 Nov 2023 17:36:41 +0900 Subject: [PATCH 04/18] perf(motion_utils): faster removeOverlapPoints and calcLateralOffset functions (#5385) Signed-off-by: Maxime CLEMENT --- .../motion_utils/trajectory/trajectory.hpp | 1 + .../benchmark_calcLateralOffset.cpp | 77 +++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 61dc65c5ea3f2..f4f9f45935347 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -168,6 +168,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) } T dst; + dst.reserve(points.size()); for (size_t i = 0; i <= start_idx; ++i) { dst.push_back(points.at(i)); diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp new file mode 100644 index 0000000000000..549ca4c0ae5bb --- /dev/null +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -0,0 +1,77 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.points.push_back(p); + } + + return traj; +} +} // namespace + +TEST(trajectory_benchmark, DISABLED_calcLateralOffset) +{ + std::random_device r; + std::default_random_engine e1(r()); + std::uniform_real_distribution uniform_dist(0.0, 1000.0); + + using motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); + constexpr auto nb_iteration = 10000; + for (auto i = 0; i < nb_iteration; ++i) { + const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0); + calcLateralOffset(traj.points, point); + } +} From ff255eb62ff8780db527cd2d3f2355997c7ad770 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Nov 2023 19:20:43 +0900 Subject: [PATCH 05/18] refactor(goal_planner): separate thread safe data (#5493) * refactor(goal_planner): separate thread safe data Signed-off-by: kosuke55 * fix style(pre-commit): autofix fix fix --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 190 ++++++--- .../goal_planner/pull_over_planner_base.hpp | 22 ++ .../goal_planner/goal_planner_module.cpp | 360 +++++++++--------- 3 files changed, 344 insertions(+), 228 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index df21bad862cc9..e4044d7191805 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -68,38 +68,30 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; -enum class PathType { - NONE = 0, - SHIFT, - ARC_FORWARD, - ARC_BACKWARD, - FREESPACE, -}; +#define DEFINE_SETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + NAME##_ = value; \ + } -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } \ - \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ +#define DEFINE_GETTER(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + return NAME##_; \ } +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ + DEFINE_SETTER(TYPE, NAME) \ + DEFINE_GETTER(TYPE, NAME) + class PullOverStatus { public: - explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} - // Reset all data members to their initial states void reset() { - std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; lane_parking_pull_over_path_ = nullptr; current_path_idx_ = 0; require_increment_ = true; @@ -109,16 +101,12 @@ class PullOverStatus pull_over_lanes_.clear(); lanes_.clear(); has_decided_path_ = false; - is_safe_static_objects_ = false; is_safe_dynamic_objects_ = false; - prev_is_safe_ = false; + prev_found_path_ = false; prev_is_safe_dynamic_objects_ = false; has_decided_velocity_ = false; - has_requested_approval_ = false; - is_ready_ = false; } - DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) DEFINE_SETTER_GETTER(size_t, current_path_idx) DEFINE_SETTER_GETTER(bool, require_increment) @@ -128,24 +116,14 @@ class PullOverStatus DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) DEFINE_SETTER_GETTER(std::vector, lanes) DEFINE_SETTER_GETTER(bool, has_decided_path) - DEFINE_SETTER_GETTER(bool, is_safe_static_objects) DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) - DEFINE_SETTER_GETTER(bool, prev_is_safe) + DEFINE_SETTER_GETTER(bool, prev_found_path) DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) DEFINE_SETTER_GETTER(bool, has_decided_velocity) - DEFINE_SETTER_GETTER(bool, has_requested_approval) - DEFINE_SETTER_GETTER(bool, is_ready) - DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) - DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) - DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) - DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) - DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER(std::optional, closest_start_pose) private: - std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; size_t current_path_idx_{0}; bool require_increment_{true}; @@ -155,32 +133,134 @@ class PullOverStatus lanelet::ConstLanelets pull_over_lanes_{}; std::vector lanes_{}; bool has_decided_path_{false}; - bool is_safe_static_objects_{false}; bool is_safe_dynamic_objects_{false}; - bool prev_is_safe_{false}; + bool prev_found_path_{false}; bool prev_is_safe_dynamic_objects_{false}; bool has_decided_velocity_{false}; - bool has_requested_approval_{false}; - bool is_ready_{false}; - // save last time and pose - std::shared_ptr last_increment_time_; - std::shared_ptr last_path_update_time_; - - // goal modification - std::optional modified_goal_pose_; Pose refined_goal_pose_{}; - GoalCandidates goal_candidates_{}; Pose closest_goal_candidate_pose_{}; +}; + +#undef DEFINE_SETTER +#undef DEFINE_GETTER +#undef DEFINE_SETTER_GETTER + +#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } + +#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) + +class ThreadSafeData +{ +public: + ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) + : mutex_(mutex), clock_(clock) + { + } + + bool incrementPathIndex() + { + const std::lock_guard lock(mutex_); + if (pull_over_path_->incrementPathIndex()) { + last_path_idx_increment_time_ = clock_->now(); + return true; + } + return false; + } + + void set_pull_over_path(const PullOverPath & value) + { + const std::lock_guard lock(mutex_); + pull_over_path_ = std::make_shared(value); + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path(const std::shared_ptr & value) + { + const std::lock_guard lock(mutex_); + pull_over_path_ = value; + last_path_update_time_ = clock_->now(); + } + + void clearPullOverPath() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + } + + bool foundPullOverPath() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + return pull_over_path_->isValidPath(); + } + + PullOverPlannerType getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return PullOverPlannerType::NONE; + } + + return pull_over_path_->type; + }; - // pull over path + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + pull_over_path_candidates_.clear(); + goal_candidates_.clear(); + modified_goal_pose_ = std::nullopt; + last_path_update_time_ = std::nullopt; + last_path_idx_increment_time_ = std::nullopt; + closest_start_pose_ = std::nullopt; + } + + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) + + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + +private: + std::shared_ptr pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; - std::optional closest_start_pose_; + GoalCandidates goal_candidates_{}; + std::optional modified_goal_pose_; + std::optional last_path_update_time_; + std::optional last_path_idx_increment_time_; + std::optional closest_start_pose_{}; std::recursive_mutex & mutex_; + rclcpp::Clock::SharedPtr clock_; }; -#undef DEFINE_SETTER_GETTER +#undef DEFINE_SETTER_WITH_MUTEX +#undef DEFINE_GETTER_WITH_MUTEX +#undef DEFINE_SETTER_GETTER_WITH_MUTEX struct FreespacePlannerDebugData { @@ -276,6 +356,7 @@ class GoalPlannerModule : public SceneModuleInterface std::recursive_mutex mutex_; PullOverStatus status_; + ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; @@ -329,6 +410,7 @@ class GoalPlannerModule : public SceneModuleInterface bool isStuck(); bool hasDecidedPath() const; void decideVelocity(); + bool foundPullOverPath() const; // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; @@ -352,8 +434,6 @@ class GoalPlannerModule : public SceneModuleInterface const GoalCandidates & goal_candidates) const; // deal with pull over partial paths - PathWithLaneId getCurrentPath() const; - bool incrementPathIndex(); void transitionToNextPathIfFinishingCurrentPath(); // lanes and drivable area diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 54bba6aee2fc2..4267261fdfe84 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -46,6 +46,7 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + size_t path_idx{0}; // accelerate with constant acceleration to the target velocity std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; @@ -84,6 +85,27 @@ struct PullOverPath return parking_path; } + + PathWithLaneId getCurrentPath() const + { + if (partial_paths.empty()) { + return PathWithLaneId{}; + } else if (partial_paths.size() <= path_idx) { + return partial_paths.back(); + } + return partial_paths.at(path_idx); + } + + bool incrementPathIndex() + { + if (partial_paths.size() - 1 <= path_idx) { + return false; + } + path_idx += 1; + return true; + } + + bool isValidPath() const { return type != PullOverPlannerType::NONE; } }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 69c1fb911c4f9..0475cd92f1bec 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -58,7 +58,7 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, - status_{mutex_} + thread_safe_data_{mutex_, clock_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -138,16 +138,12 @@ void GoalPlannerModule::updateOccupancyGrid() void GoalPlannerModule::onTimer() { // already generated pull over candidate paths - if (!status_.get_pull_over_path_candidates().empty()) { + if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { return; } // goals are not yet available. - if (status_.get_goal_candidates().empty()) { - return; - } - - if (!isExecutionRequested()) { + if (thread_safe_data_.get_goal_candidates().empty()) { return; } @@ -157,7 +153,11 @@ void GoalPlannerModule::onTimer() return; } - const auto goal_candidates = status_.get_goal_candidates(); + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + + const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( @@ -208,8 +208,8 @@ void GoalPlannerModule::onTimer() // set member variables { const std::lock_guard lock(mutex_); - status_.set_pull_over_path_candidates(path_candidates); - status_.set_closest_start_pose(closest_start_pose); + thread_safe_data_.set_pull_over_path_candidates(path_candidates); + thread_safe_data_.set_closest_start_pose(closest_start_pose); } } @@ -457,13 +457,13 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const bool GoalPlannerModule::planFreespacePath() { - goal_searcher_->setPlannerData(planner_data_); - auto goal_candidates = status_.get_goal_candidates(); - goal_searcher_->update(goal_candidates); - status_.set_goal_candidates(goal_candidates); - + GoalCandidates goal_candidates{}; { const std::lock_guard lock(mutex_); + goal_searcher_->setPlannerData(planner_data_); + goal_candidates = thread_safe_data_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + thread_safe_data_.set_goal_candidates(goal_candidates); debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); debug_data_.freespace_planner.is_planning = true; } @@ -487,11 +487,8 @@ bool GoalPlannerModule::planFreespacePath() { const std::lock_guard lock(mutex_); - status_.set_pull_over_path(std::make_shared(*freespace_path)); - status_.set_current_path_idx(0); - status_.set_is_safe_static_objects(true); - status_.set_modified_goal_pose(goal_candidate); - status_.set_last_path_update_time(std::make_shared(clock_->now())); + thread_safe_data_.set_pull_over_path(*freespace_path); + thread_safe_data_.set_modified_goal_pose(goal_candidate); debug_data_.freespace_planner.is_planning = false; } @@ -527,14 +524,8 @@ void GoalPlannerModule::returnToLaneParking() return; } - { - const std::lock_guard lock(mutex_); - status_.set_is_safe_static_objects(true); - status_.set_has_decided_path(false); - status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); - status_.set_current_path_idx(0); - status_.set_last_path_update_time(std::make_shared(clock_->now())); - } + status_.set_has_decided_path(false); + thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -546,7 +537,7 @@ void GoalPlannerModule::generateGoalCandidates() // with old architecture, module instance is not cleared when new route is received // so need to reset status here. // todo: move this check out of this function after old architecture is removed - if (!status_.get_goal_candidates().empty()) { + if (!thread_safe_data_.get_goal_candidates().empty()) { return; } @@ -554,21 +545,23 @@ void GoalPlannerModule::generateGoalCandidates() const Pose goal_pose = route_handler->getOriginalGoalPose(); status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); - status_.set_goal_candidates(goal_searcher_->search()); + thread_safe_data_.set_goal_candidates(goal_searcher_->search()); const auto current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, false); status_.set_closest_goal_candidate_pose( - goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose); + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) + .goal_pose); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; GoalCandidates goal_candidates{}; goal_candidates.push_back(goal_candidate); - status_.set_goal_candidates(goal_candidates); + thread_safe_data_.set_goal_candidates(goal_candidates); status_.set_closest_goal_candidate_pose(goal_pose); } } @@ -630,13 +623,13 @@ void GoalPlannerModule::selectSafePullOverPath() { const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); - goal_candidates = status_.get_goal_candidates(); + goal_candidates = thread_safe_data_.get_goal_candidates(); goal_searcher_->update(goal_candidates); - status_.set_goal_candidates(goal_candidates); - status_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( - status_.get_pull_over_path_candidates(), status_.get_goal_candidates())); - pull_over_path_candidates = status_.get_pull_over_path_candidates(); - status_.set_is_safe_static_objects(false); + thread_safe_data_.set_goal_candidates(goal_candidates); + thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( + thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates())); + pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); + thread_safe_data_.clearPullOverPath(); } for (const auto & pull_over_path : pull_over_path_candidates) { @@ -660,26 +653,24 @@ void GoalPlannerModule::selectSafePullOverPath() // found safe pull over path { const std::lock_guard lock(mutex_); - status_.set_is_safe_static_objects(true); - status_.set_pull_over_path(std::make_shared(pull_over_path)); - status_.set_current_path_idx(0); - status_.set_lane_parking_pull_over_path(status_.get_pull_over_path()); - status_.set_modified_goal_pose(*goal_candidate_it); - status_.set_last_path_update_time(std::make_shared(clock_->now())); + thread_safe_data_.set_pull_over_path(pull_over_path); + thread_safe_data_.set_modified_goal_pose(*goal_candidate_it); + status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path()); } break; } - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return; } // decelerate before the search area start const auto search_start_offset_pose = calcLongitudinalOffsetPose( - status_.get_pull_over_path()->getFullPath().points, status_.get_refined_goal_pose().position, + thread_safe_data_.get_pull_over_path()->getFullPath().points, + status_.get_refined_goal_pose().position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - approximate_pull_over_distance_); - auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); if (search_start_offset_pose) { decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { @@ -714,7 +705,7 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path setStopPath(output); } else if ( @@ -732,7 +723,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) } // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); + auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); output.path = std::make_shared(current_path); @@ -751,14 +742,14 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. const std::lock_guard lock(mutex_); - status_.set_prev_is_safe(status_.get_is_safe_static_objects()); + status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); status_.set_prev_is_safe_dynamic_objects( parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { - if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { + if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); const std::lock_guard lock(mutex_); @@ -766,10 +757,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) // set stop path as pull over path auto stop_pull_over_path = std::make_shared(); stop_pull_over_path->partial_paths.push_back(*output.path); - status_.set_pull_over_path(stop_pull_over_path); - status_.set_current_path_idx(0); - status_.set_last_path_update_time(std::make_shared(clock_->now())); - + thread_safe_data_.set_pull_over_path(stop_pull_over_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { @@ -786,7 +774,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { - auto current_path = getCurrentPath(); + auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, @@ -796,12 +784,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { - output.path = std::make_shared(getCurrentPath()); + output.path = + std::make_shared(thread_safe_data_.get_pull_over_path()->getCurrentPath()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - status_.set_last_path_update_time(std::make_shared(clock_->now())); + // status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.get_prev_stop_path_after_approval(); @@ -813,7 +802,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -831,10 +820,10 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.get_is_safe_static_objects()) { + if (thread_safe_data_.foundPullOverPath()) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = status_.get_modified_goal_pose()->goal_pose; + modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -879,22 +868,25 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return false; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose, + std::numeric_limits::max(), M_PI_2); if (!ego_segment_idx) { return false; } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.get_pull_over_path()->start_pose.position); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, + thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, + *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, + start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -904,7 +896,7 @@ void GoalPlannerModule::decideVelocity() // decide velocity to guarantee turn signal lighting time if (!status_.get_has_decided_velocity()) { - auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { @@ -917,13 +909,14 @@ void GoalPlannerModule::decideVelocity() CandidateOutput GoalPlannerModule::planCandidate() const { return CandidateOutput( - status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); + thread_safe_data_.get_pull_over_path() ? thread_safe_data_.get_pull_over_path()->getFullPath() + : PathWithLaneId()); } BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output - if (status_.get_pull_over_path_candidates().empty()) { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); } @@ -944,10 +937,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() } transitionToNextPathIfFinishingCurrentPath(); } else if ( - !status_.get_pull_over_path_candidates().empty() && needPathUpdate(path_update_duration)) { + !thread_safe_data_.get_pull_over_path_candidates().empty() && + needPathUpdate(path_update_duration)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to status_.get_pull_over_path() + // and set it to thread_safe_data_.get_pull_over_path() selectSafePullOverPath(); } // else: stop path is generated and set by setOutput() @@ -959,27 +953,32 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } + // For debug + setDebugData(); + if (parameters_->print_debug_info) { + // For evaluations + printParkingPositionError(); + } + + if (!thread_safe_data_.foundPullOverPath()) { + return output; + } + const auto distance_to_path_change = calcDistanceToPathChange(); if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING updateSteeringFactor( - {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, + {thread_safe_data_.get_pull_over_path()->start_pose, + thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); - // For debug - setDebugData(); - if (parameters_->print_debug_info) { - // For evaluations - printParkingPositionError(); - } - - setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); return output; } @@ -1002,7 +1001,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output - if (status_.get_pull_over_path_candidates().empty()) { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); } @@ -1012,10 +1011,9 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; - const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1029,21 +1027,31 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } + if (!thread_safe_data_.foundPullOverPath()) { + return out; + } + + const auto distance_to_path_change = calcDistanceToPathChange(); if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, + {thread_safe_data_.get_pull_over_path()->start_pose, + thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); return out; } std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto full_path = status_.get_pull_over_path()->getFullPath(); + if (!thread_safe_data_.foundPullOverPath()) { + return {std::numeric_limits::max(), std::numeric_limits::max()}; + } + + const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1053,15 +1061,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.get_pull_over_path()->start_pose.position); + full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.get_modified_goal_pose()->goal_pose.position); + full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); + thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1103,17 +1111,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, status_.get_closest_goal_candidate_pose().position, -approximate_pull_over_distance_); if ( - !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && + !thread_safe_data_.foundPullOverPath() && !thread_safe_data_.get_closest_start_pose() && !search_start_offset_pose) { return generateFeasibleStopPath(); } const Pose stop_pose = [&]() -> Pose { - if (status_.get_is_safe_static_objects()) { - return status_.get_pull_over_path()->start_pose; + if (thread_safe_data_.foundPullOverPath()) { + return thread_safe_data_.get_pull_over_path()->start_pose; } - if (status_.get_closest_start_pose()) { - return status_.get_closest_start_pose().value(); + if (thread_safe_data_.get_closest_start_pose()) { + return thread_safe_data_.get_closest_start_pose().value(); } return *search_start_offset_pose; }(); @@ -1187,44 +1195,42 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && last_approval_data_) { - // if using arc_path and finishing current_path, get next path - // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() > - planner_data_->parameters.turn_signal_search_time; - - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { - if (incrementPathIndex()) { - status_.set_last_increment_time(std::make_shared(clock_->now())); - } - } + if (!isActivated() || !last_approval_data_) { + return; } -} -bool GoalPlannerModule::incrementPathIndex() -{ - if (status_.get_current_path_idx() == status_.get_pull_over_path()->partial_paths.size() - 1) { - return false; + // if using arc_path and finishing current_path, get next path + // enough time for turn signal + const bool has_passed_enough_time_from_approval = + (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; + if (!has_passed_enough_time_from_approval) { + return; } - status_.set_current_path_idx(status_.get_current_path_idx() + 1); - return true; -} -PathWithLaneId GoalPlannerModule::getCurrentPath() const -{ - if (status_.get_pull_over_path() == nullptr) { - return PathWithLaneId{}; + // require increment only when the time passed is enough + // to prevent increment before driving + // when the end of the current path is close to the current pose + // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + constexpr double keep_current_idx_time = 4.0; + const bool has_passed_enough_time_from_increment = + (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > + keep_current_idx_time; + if (!has_passed_enough_time_from_increment) { + return; } - if (status_.get_pull_over_path()->partial_paths.size() <= status_.get_current_path_idx()) { - return PathWithLaneId{}; + if (!hasFinishedCurrentPath()) { + return; } - return status_.get_pull_over_path()->partial_paths.at(status_.get_current_path_idx()); + + thread_safe_data_.incrementPathIndex(); } bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { + const std::lock_guard lock(mutex_); odometry_buffer.push_back(planner_data_->self_odometry); // Delete old data in buffer while (rclcpp::ok()) { @@ -1254,6 +1260,7 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + const std::lock_guard lock(mutex_); if (isOnModifiedGoal()) { return false; } @@ -1264,21 +1271,22 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return true; } // any path has never been found - if (!status_.get_pull_over_path()) { + if (!thread_safe_data_.get_pull_over_path()) { return false; } - return checkCollision(getCurrentPath()); + return checkCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto current_path_end = getCurrentPath().points.back(); + const auto current_path_end = + thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1288,12 +1296,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnModifiedGoal() const { - if (!status_.get_modified_goal_pose()) { + if (!thread_safe_data_.get_modified_goal_pose()) { return false; } const Pose current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, status_.get_modified_goal_pose()->goal_pose) < + return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < parameters_->th_arrived_distance; } @@ -1302,9 +1310,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.get_pull_over_path()->start_pose; - const auto & end_pose = status_.get_pull_over_path()->end_pose; - const auto full_path = status_.get_pull_over_path()->getFullPath(); + const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; + const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); // calc TurnIndicatorsCommand { @@ -1435,20 +1443,18 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; - constexpr double keep_current_idx_buffer_time = 2.0; - if (status_.get_last_increment_time()) { - const auto time_diff = (clock_->now() - *status_.get_last_increment_time()).seconds(); - if (time_diff < keep_stop_time) { - status_.set_require_increment(false); - for (auto & p : path.points) { - p.point.longitudinal_velocity_mps = 0.0; - } - } else if (time_diff > keep_stop_time + keep_current_idx_buffer_time) { - // require increment only when the time passed is enough - // to prevent increment before driving - // when the end of the current path is close to the current pose - status_.set_require_increment(true); - } + if (!thread_safe_data_.get_last_path_idx_increment_time()) { + return; + } + + const auto time_diff = + (clock_->now() - *thread_safe_data_.get_last_path_idx_increment_time()).seconds(); + if (time_diff > keep_stop_time) { + return; + } + + for (auto & p : path.points) { + p.point.longitudinal_velocity_mps = 0.0; } } @@ -1499,7 +1505,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { - const auto stop_point = utils::insertStopPoint(stop_point_length, path); + utils::insertStopPoint(stop_point_length, path); } } @@ -1631,7 +1637,7 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( bool GoalPlannerModule::isSafePath() const { - const auto pull_over_path = getCurrentPath(); + const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data_->self_odometry->twist.twist.linear.x, @@ -1648,7 +1654,7 @@ bool GoalPlannerModule::isSafePath() const const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( - status_.get_pull_over_path()->pairs_terminal_velocity_and_accel, + thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, status_.get_current_path_idx()); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", @@ -1744,23 +1750,25 @@ void GoalPlannerModule::setDebugData() goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - const auto goal_candidates = status_.get_goal_candidates(); + const auto goal_candidates = thread_safe_data_.get_goal_candidates(); add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } // Visualize path and related pose - if (status_.get_is_safe_static_objects()) { + if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( - status_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, + 0.9)); add(createPoseMarkerArray( - status_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray( + thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray( - status_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < status_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = status_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { + const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -1782,7 +1790,15 @@ void GoalPlannerModule::setDebugData() } } debug_marker_.markers.push_back(marker); + + // Visualize debug poses + const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; + for (size_t i = 0; i < debug_poses.size(); ++i) { + add(createPoseMarkerArray( + debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); + } } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (goal_planner_data_.ego_predicted_path.size() > 0) { @@ -1806,17 +1822,22 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.get_is_safe_static_objects() + const auto color = thread_safe_data_.foundPullOverPath() ? createMarkerColor(1.0, 1.0, 1.0, 0.99) : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = status_.get_modified_goal_pose() ? status_.get_modified_goal_pose()->goal_pose - : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(status_.get_pull_over_path()->type); - marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + - std::to_string(status_.get_pull_over_path()->partial_paths.size() - 1); + marker.pose = thread_safe_data_.get_modified_goal_pose() + ? thread_safe_data_.get_modified_goal_pose()->goal_pose + : planner_data_->self_odometry->pose.pose; + marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); + if (thread_safe_data_.foundPullOverPath()) { + marker.text += + " " + std::to_string(status_.get_current_path_idx()) + "/" + + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); + } + if (isStuck()) { marker.text += " stuck"; } else if (isStopped()) { @@ -1832,13 +1853,6 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } - - // Visualize debug poses - const auto & debug_poses = status_.get_pull_over_path()->debug_poses; - for (size_t i = 0; i < debug_poses.size(); ++i) { - add(createPoseMarkerArray( - debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); - } } void GoalPlannerModule::printParkingPositionError() const @@ -1847,7 +1861,7 @@ void GoalPlannerModule::printParkingPositionError() const const double real_shoulder_to_map_shoulder = 0.0; const Pose goal_to_ego = - inverseTransformPose(current_pose, status_.get_modified_goal_pose()->goal_pose); + inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose); const double dx = goal_to_ego.position.x; const double dy = goal_to_ego.position.y; const double distance_from_real_shoulder = @@ -1856,7 +1870,7 @@ void GoalPlannerModule::printParkingPositionError() const getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, tier4_autoware_utils::rad2deg( tf2::getYaw(current_pose.orientation) - - tf2::getYaw(status_.get_modified_goal_pose()->goal_pose.orientation)), + tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); } @@ -1882,10 +1896,10 @@ bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { - if (!status_.get_last_path_update_time()) { + if (!thread_safe_data_.get_last_path_update_time()) { return true; } - return (clock_->now() - *status_.get_last_path_update_time()).seconds() > duration; + return (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > duration; } } // namespace behavior_path_planner From 08b905aedd4def7dd04c9d3ae486f361179c87c2 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 10 Nov 2023 19:21:20 +0900 Subject: [PATCH 06/18] refactor(start_planner): refactor backward path calculation in StartPlannerModule (#5529) * refactor(start_planner): refactor backward path calculation in StartPlannerModule The method "calcStartPoseCandidatesBackwardPath" has been renamed to "calcBackwardPathFromStartPose" to better reflect its purpose. The method now calculates the backward path by shifting the original start pose coordinates to align with the pull out lanes. The stop objects in the pull out lanes are filtered by velocity, using the new "filterStopObjectsInPullOutLanes" method. Additionally, the redundant "isOverlappedWithLane" method has been removed. Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- ...avior_path_planner_start_planner_design.md | 2 +- .../start_planner/start_planner_module.hpp | 10 +- .../start_planner/start_planner_module.cpp | 101 +++++++++--------- 3 files changed, 55 insertions(+), 58 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 131202038c270..28a1db649cf04 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -252,7 +252,7 @@ If a safe path cannot be generated from the current position, search backwards f | max_back_distance | [m] | double | maximum back distance | 30.0 | | backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | | backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | -| ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | +| ignore_distance_from_lane_end | [m] | double | If distance from shift start pose to end of shoulder lane is less than this value, this start pose candidate is ignored | 15.0 | ### **freespace pull out** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 91121598cb3ae..8d2532504926e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -176,8 +176,9 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - PathWithLaneId calcStartPoseCandidatesBackwardPath() const; - std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; + PathWithLaneId calcBackwardPathFromStartPose() const; + std::vector searchPullOutStartPoseCandidates( + const PathWithLaneId & back_path_from_start_pose) const; std::shared_ptr lane_departure_checker_; @@ -194,9 +195,8 @@ class StartPlannerModule : public SceneModuleInterface std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); - static bool isOverlappedWithLane( - const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint); + PredictedObjects filterStopObjectsInPullOutLanes( + const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; bool hasFinishedPullOut() const; bool isBackwardDrivingComplete() const; bool isStopped(); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 77a290835fb31..75562f59a402f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -718,7 +718,7 @@ void StartPlannerModule::updatePullOutStatus() // refine start pose with pull out lanes. // 1) backward driving is not allowed: use refined pose just as start pose. // 2) backward driving is allowed: use refined pose to check if backward driving is needed. - const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose(); const auto refined_start_pose = calcLongitudinalOffsetPose( start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); if (!refined_start_pose) return; @@ -726,7 +726,7 @@ void StartPlannerModule::updatePullOutStatus() // search pull out start candidates backward const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { if (parameters_->enable_back) { - return searchPullOutStartPoses(start_pose_candidates_path); + return searchPullOutStartPoseCandidates(start_pose_candidates_path); } return {*refined_start_pose}; }); @@ -739,6 +739,7 @@ void StartPlannerModule::updatePullOutStatus() if (isBackwardDrivingComplete()) { updateStatusAfterBackwardDriving(); + // should be moved to transition state current_state_ = ModuleStatus::SUCCESS; // for breaking loop } else { status_.backward_path = start_planner_utils::getBackwardPath( @@ -760,71 +761,66 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() } } -PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const +PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - // get backward shoulder path - const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto path = planner_data_->route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - check_distance, - arc_position_pose.length + check_distance); - - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose); + + // common buffer distance for both front and back + static constexpr double buffer = 30.0; + const double check_distance = parameters_->max_back_distance + buffer; + + const double start_distance = arc_position_pose.length - check_distance; + const double end_distance = arc_position_pose.length + buffer; + + auto path = + planner_data_->route_handler->getCenterLinePath(pull_out_lanes, start_distance, end_distance); + + // shift all path points laterally to align with the start pose + for (auto & path_point : path.points) { + path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0); } return path; } -std::vector StartPlannerModule::searchPullOutStartPoses( - const PathWithLaneId & start_pose_candidates) const +std::vector StartPlannerModule::searchPullOutStartPoseCandidates( + const PathWithLaneId & back_path_from_start_pose) const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - - std::vector pull_out_start_pose{}; - + std::vector pull_out_start_pose_candidates{}; + const auto & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - // filter pull out lanes stop objects - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, pull_out_lanes, - utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_->th_moving_object_velocity); + const auto stop_objects_in_pull_out_lanes = + filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity); // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. - const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose).length; - const double max_back_distance = std::clamp( - s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); + const double current_arc_length = + lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; + const double allowed_backward_distance = std::clamp( + current_arc_length - planner_data_->parameters.base_link2rear, 0.0, + parameters_->max_back_distance); - // check collision between footprint and object at the backed pose - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= max_back_distance; + for (double back_distance = 0.0; back_distance <= allowed_backward_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - start_pose_candidates.points, current_pose.position, -back_distance); + back_path_from_start_pose.points, start_pose.position, -back_distance); if (!backed_pose) { continue; } - // check the back pose is near the lane end - const double length_to_backed_pose = + const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length; - const double length_to_lane_end = std::accumulate( std::begin(pull_out_lanes), std::end(pull_out_lanes), 0.0, [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); - const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; + const double distance_from_lane_end = length_to_lane_end - backed_pose_arc_length; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, @@ -833,27 +829,28 @@ std::vector StartPlannerModule::searchPullOutStartPoses( } if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, pull_out_lane_stop_objects, + local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, parameters_->collision_check_margin)) { break; // poses behind this has a collision, so break. } - pull_out_start_pose.push_back(*backed_pose); + pull_out_start_pose_candidates.push_back(*backed_pose); } - return pull_out_start_pose; + return pull_out_start_pose_candidates; } -bool StartPlannerModule::isOverlappedWithLane( - const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) +PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( + const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const { - for (const auto & point : vehicle_footprint) { - if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) { - return true; - } - } + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *planner_data_->dynamic_object, velocity_threshold); - return false; + // filter for objects located in pull_out_lanes and moving at a speed below the threshold + const auto [stop_objects_in_pull_out_lanes, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + return stop_objects_in_pull_out_lanes; } bool StartPlannerModule::hasFinishedPullOut() const From 208ecf21240ebca1802903e079855f7b6ab6bb11 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Fri, 10 Nov 2023 13:25:24 +0300 Subject: [PATCH 07/18] build(lidar_centerpoint_tvm): remove artifacts download (#5367) Signed-off-by: Alexey Panferov --- perception/lidar_centerpoint_tvm/.gitignore | 1 - .../inference_engine_tvm_config.hpp | 60 +++++++++++++++++++ ...processing_inference_engine_tvm_config.hpp | 59 ++++++++++++++++++ .../inference_engine_tvm_config.hpp | 55 +++++++++++++++++ .../lidar_centerpoint_tvm/centerpoint_tvm.hpp | 5 +- .../launch/lidar_centerpoint_tvm.launch.xml | 2 + ...inference_lidar_centerpoint_tvm.launch.xml | 2 + .../lib/centerpoint_tvm.cpp | 14 ++--- perception/lidar_centerpoint_tvm/src/node.cpp | 3 +- .../src/single_inference_node.cpp | 4 +- 10 files changed, 192 insertions(+), 13 deletions(-) create mode 100644 perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp create mode 100644 perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp create mode 100644 perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp diff --git a/perception/lidar_centerpoint_tvm/.gitignore b/perception/lidar_centerpoint_tvm/.gitignore index 8fce603003c1e..e69de29bb2d1d 100644 --- a/perception/lidar_centerpoint_tvm/.gitignore +++ b/perception/lidar_centerpoint_tvm/.gitignore @@ -1 +0,0 @@ -data/ diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..8201dd25c1039 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp @@ -0,0 +1,60 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT + +namespace model_zoo +{ +namespace perception +{ +namespace lidar_obstacle_detection +{ +namespace centerpoint_backbone +{ +namespace onnx_centerpoint_backbone +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {3, 0, 0}, // modelzoo_version + + "centerpoint_backbone", // network_name + "llvm", // network_backend + + "./deploy_lib.so", // network_module_path + "./deploy_graph.json", // network_graph_path + "./deploy_param.params", // network_params_path + + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"spatial_features", kDLFloat, 32, 1, {1, 32, 560, 560}}}, // network_inputs + + {{"heatmap", kDLFloat, 32, 1, {1, 3, 560, 560}}, + {"reg", kDLFloat, 32, 1, {1, 2, 560, 560}}, + {"height", kDLFloat, 32, 1, {1, 1, 560, 560}}, + {"dim", kDLFloat, 32, 1, {1, 3, 560, 560}}, + {"rot", kDLFloat, 32, 1, {1, 2, 560, 560}}, + {"vel", kDLFloat, 32, 1, {1, 2, 560, 560}}} // network_outputs +}; + +} // namespace onnx_centerpoint_backbone +} // namespace centerpoint_backbone +} // namespace lidar_obstacle_detection +} // namespace perception +} // namespace model_zoo +// NOLINTNEXTLINE +#endif // PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__INFERENCE_ENGINE_TVM_CONFIG_HPP_ diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..2f6943e90bc83 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp @@ -0,0 +1,59 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__PREPROCESSING_INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__PREPROCESSING_INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT + +namespace model_zoo +{ +namespace perception +{ +namespace lidar_obstacle_detection +{ +namespace centerpoint_backbone +{ +namespace onnx_centerpoint_backbone +{ +namespace preprocessing +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {3, 0, 0}, // modelzoo_version + + "centerpoint_backbone", // network_name + "llvm", // network_backend + + "./preprocess.so", // network_module_path + "./", // network_graph_path + "./", // network_params_path + + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"pillar_features", kDLFloat, 32, 1, {40000, 1, 32}}, + {"coords", kDLInt, 32, 1, {40000, 3}}}, // network_inputs + + {{"spatial_features", kDLFloat, 32, 1, {1, 32, 560, 560}}} // network_outputs +}; + +} // namespace preprocessing +} // namespace onnx_centerpoint_backbone +} // namespace centerpoint_backbone +} // namespace lidar_obstacle_detection +} // namespace perception +} // namespace model_zoo +// NOLINTNEXTLINE +#endif // PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_BACKBONE__PREPROCESSING_INFERENCE_ENGINE_TVM_CONFIG_HPP_ diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..521036b49a533 --- /dev/null +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp @@ -0,0 +1,55 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_ENCODER__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_ENCODER__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT + +namespace model_zoo +{ +namespace perception +{ +namespace lidar_obstacle_detection +{ +namespace centerpoint_encoder +{ +namespace onnx_centerpoint_encoder +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {3, 0, 0}, // modelzoo_version + + "centerpoint_encoder", // network_name + "llvm", // network_backend + + "./deploy_lib.so", // network_module_path + "./deploy_graph.json", // network_graph_path + "./deploy_param.params", // network_params_path + + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"input_features", kDLFloat, 32, 1, {40000, 32, 9}}}, // network_inputs + + {{"pillar_features", kDLFloat, 32, 1, {40000, 1, 32}}} // network_outputs +}; + +} // namespace onnx_centerpoint_encoder +} // namespace centerpoint_encoder +} // namespace lidar_obstacle_detection +} // namespace perception +} // namespace model_zoo +// NOLINTNEXTLINE +#endif // PERCEPTION__LIDAR_CENTERPOINT_TVM__DATA__MODELS__CENTERPOINT_ENCODER__INFERENCE_ENGINE_TVM_CONFIG_HPP_ diff --git a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/centerpoint_tvm.hpp b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/centerpoint_tvm.hpp index 168a249714bb4..865ca7d4253bf 100644 --- a/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/centerpoint_tvm.hpp +++ b/perception/lidar_centerpoint_tvm/include/lidar_centerpoint_tvm/centerpoint_tvm.hpp @@ -60,7 +60,7 @@ class LIDAR_CENTERPOINT_TVM_LOCAL TVMScatterIE : public tvm_utility::pipeline::I public: explicit TVMScatterIE( tvm_utility::pipeline::InferenceEngineTVMConfig config, const std::string & pkg_name, - const std::string & function_name); + const std::string & data_path, const std::string & function_name); TVMArrayContainerVector schedule(const TVMArrayContainerVector & input); void set_coords(TVMArrayContainer coords) { coords_ = coords; } @@ -132,7 +132,8 @@ class LIDAR_CENTERPOINT_TVM_PUBLIC CenterPointTVM /// \param[in] dense_param The densification parameter used to constructing vg_ptr. /// \param[in] config The CenterPoint model configuration. explicit CenterPointTVM( - const DensificationParam & densification_param, const CenterPointConfig & config); + const DensificationParam & densification_param, const CenterPointConfig & config, + const std::string & data_path); ~CenterPointTVM(); diff --git a/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml b/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml index e838f01e347ce..2bd6e3aa15c21 100644 --- a/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml +++ b/perception/lidar_centerpoint_tvm/launch/lidar_centerpoint_tvm.launch.xml @@ -10,6 +10,7 @@ + @@ -20,5 +21,6 @@ + diff --git a/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml index e62808804ca07..c2e0801fbd11c 100644 --- a/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml +++ b/perception/lidar_centerpoint_tvm/launch/single_inference_lidar_centerpoint_tvm.launch.xml @@ -12,6 +12,7 @@ + @@ -22,6 +23,7 @@ + diff --git a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp index 7b31bdf3c8fd3..2e0f9ad28bfb6 100644 --- a/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp +++ b/perception/lidar_centerpoint_tvm/lib/centerpoint_tvm.cpp @@ -43,11 +43,10 @@ namespace lidar_centerpoint_tvm TVMScatterIE::TVMScatterIE( tvm_utility::pipeline::InferenceEngineTVMConfig config, const std::string & pkg_name, - const std::string & function_name) + const std::string & data_path, const std::string & function_name) : config_(config) { - std::string network_prefix = - ament_index_cpp::get_package_share_directory(pkg_name) + "/models/" + config.network_name + "/"; + std::string network_prefix = data_path + "/" + pkg_name + "/models/" + config.network_name + "/"; std::string network_module_path = network_prefix + config.network_module_path; std::ifstream module(network_module_path); @@ -159,14 +158,15 @@ std::vector BackboneNeckHeadPostProcessor::schedule(const TVMArrayContain } CenterPointTVM::CenterPointTVM( - const DensificationParam & densification_param, const CenterPointConfig & config) + const DensificationParam & densification_param, const CenterPointConfig & config, + const std::string & data_path) : config_ve(config_en), config_bnh(config_bk), VE_PreP(std::make_shared(config_en, config)), - VE_IE(std::make_shared(config_en, "lidar_centerpoint_tvm")), - BNH_IE(std::make_shared(config_bk, "lidar_centerpoint_tvm")), + VE_IE(std::make_shared(config_en, "lidar_centerpoint_tvm", data_path)), + BNH_IE(std::make_shared(config_bk, "lidar_centerpoint_tvm", data_path)), BNH_PostP(std::make_shared(config_bk, config)), - scatter_ie(std::make_shared(config_scatter, "lidar_centerpoint_tvm", "scatter")), + scatter_ie(std::make_shared(config_scatter, "lidar_centerpoint_tvm", data_path, "scatter")), TSP_pipeline(std::make_shared(VE_PreP, VE_IE, scatter_ie, BNH_IE, BNH_PostP)), config_(config) { diff --git a/perception/lidar_centerpoint_tvm/src/node.cpp b/perception/lidar_centerpoint_tvm/src/node.cpp index e63cb7e19ba8e..0054211a15f9f 100644 --- a/perception/lidar_centerpoint_tvm/src/node.cpp +++ b/perception/lidar_centerpoint_tvm/src/node.cpp @@ -65,6 +65,7 @@ LidarCenterPointTVMNode::LidarCenterPointTVMNode(const rclcpp::NodeOptions & nod static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto data_path = this->declare_parameter("data_path"); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); @@ -83,7 +84,7 @@ LidarCenterPointTVMNode::LidarCenterPointTVMNode(const rclcpp::NodeOptions & nod class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, yaw_norm_threshold); - detector_ptr_ = std::make_unique(densification_param, config); + detector_ptr_ = std::make_unique(densification_param, config, data_path); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), diff --git a/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp index 8b810c5a7d759..a08850a7572f2 100644 --- a/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp +++ b/perception/lidar_centerpoint_tvm/src/single_inference_node.cpp @@ -70,7 +70,7 @@ SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( static_cast(this->declare_parameter("encoder_in_feature_size")); const auto pcd_path = this->declare_parameter("pcd_path"); const auto detections_path = this->declare_parameter("detections_path"); - + const auto data_path = this->declare_parameter("data_path"); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); @@ -88,7 +88,7 @@ SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, yaw_norm_threshold); - detector_ptr_ = std::make_unique(densification_param, config); + detector_ptr_ = std::make_unique(densification_param, config, data_path); detect(pcd_path, detections_path); exit(0); From 042c7a0c51ba72b8fb7b0199e292c15512b6da4b Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Sat, 11 Nov 2023 14:44:04 +0800 Subject: [PATCH 08/18] refactor(perception_rviz_plugin): apply thread pool to manage detached thread (#5418) * apply thread pool to manage detached thread Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * clean up the destructor Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * use function object in the queue instead Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix condition variable naming problem Signed-off-by: Owen-Liuyuxuan * add utility include for CI Signed-off-by: Owen-Liuyuxuan --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../predicted_objects_display.hpp | 33 ++++++++++++++ .../predicted_objects_display.cpp | 45 +++++++++++++------ 2 files changed, 64 insertions(+), 14 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 5493f1dd594ce..2896286970217 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -24,9 +24,11 @@ #include #include +#include #include #include #include +#include #include namespace autoware @@ -45,10 +47,31 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); + ~PredictedObjectsDisplay() + { + { + std::unique_lock lock(queue_mutex); + should_terminate = true; + } + condition.notify_all(); + for (std::thread & active_thread : threads) { + active_thread.join(); + } + threads.clear(); + } private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; + void queueJob(std::function job) + { + { + std::unique_lock lock(queue_mutex); + jobs.push(std::move(job)); + } + condition.notify_one(); + } + boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { const std::string uuid_str = uuid_to_string(uuid_msg); @@ -100,6 +123,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay PredictedObjects::ConstSharedPtr msg); void workerThread(); + void messageProcessorThreadJob(); + void update(float wall_dt, float ros_dt) override; std::unordered_map> id_map; @@ -108,6 +133,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay int32_t marker_id = 0; const int32_t PATH_ID_CONSTANT = 1e3; + // max_num_threads: number of threads created in the thread pool, hard-coded to be 1; + int max_num_threads; + + bool should_terminate{false}; + std::mutex queue_mutex; + std::vector threads; + std::queue> jobs; + PredictedObjects::ConstSharedPtr msg; bool consumed{false}; std::mutex mutex; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 24e67a6f44e95..2cc5397d18721 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -25,27 +25,44 @@ namespace object_detection { PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") { - std::thread worker(&PredictedObjectsDisplay::workerThread, this); - worker.detach(); + max_num_threads = 1; // hard code the number of threads to be created + + for (int ii = 0; ii < max_num_threads; ++ii) { + threads.emplace_back(std::thread(&PredictedObjectsDisplay::workerThread, this)); + } } void PredictedObjectsDisplay::workerThread() -{ +{ // A standard working thread that waiting for jobs while (true) { - std::unique_lock lock(mutex); - condition.wait(lock, [this] { return this->msg; }); + std::function job; + { + std::unique_lock lock(queue_mutex); + condition.wait(lock, [this] { return !jobs.empty() || should_terminate; }); + if (should_terminate) { + return; + } + job = jobs.front(); + jobs.pop(); + } + job(); + } +} - auto tmp_msg = this->msg; - this->msg.reset(); +void PredictedObjectsDisplay::messageProcessorThreadJob() +{ + // Receiving + std::unique_lock lock(mutex); + auto tmp_msg = this->msg; + this->msg.reset(); + lock.unlock(); - lock.unlock(); + auto tmp_markers = createMarkers(tmp_msg); - auto tmp_markers = createMarkers(tmp_msg); - lock.lock(); - markers = tmp_markers; + lock.lock(); + markers = tmp_markers; - consumed = true; - } + consumed = true; } std::vector PredictedObjectsDisplay::createMarkers( @@ -188,7 +205,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms std::unique_lock lock(mutex); this->msg = msg; - condition.notify_one(); + queueJob(std::bind(&PredictedObjectsDisplay::messageProcessorThreadJob, this)); } void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) From a84c43b2cb6c0a3dd00df19f66866d28dd15c752 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 13 Nov 2023 09:52:56 +0900 Subject: [PATCH 09/18] feat(intersection): use the centerline position of first attention area if there is no traffic light (#5547) Signed-off-by: Mamoru Sobue --- .../README.md | 15 +- .../docs/data-structure.drawio.svg | 745 ++++++++++++------ .../src/manager.cpp | 1 + .../src/scene_intersection.cpp | 173 ++-- .../src/scene_merge_from_private_road.cpp | 4 +- .../src/util.cpp | 86 +- .../src/util.hpp | 3 +- .../src/util_type.hpp | 3 +- 8 files changed, 690 insertions(+), 340 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index cb65e2a3cc23c..3ed4ee8fd334a 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -100,7 +100,18 @@ If a stopline is associated with the intersection lane on the map, that line is #### Pass Judge Line -To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, namely the `first_attention_stop_line`, this module does not insert stopline after it passed the `default stop_line` position. + +The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. + +- If `occlusion.enable` is false, the pass judge line before the `first_attention_stop_line` by the braking distance $v_{ego}^{2} / 2a_{max}$. +- If `occlusion.enable` is true and: + - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stop_line` in order to continue peeking/collision detection while occlusion is detected. + - if there are no associated traffic lights and: + - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. + - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. + +![data structure](./docs/data-structure.drawio.svg) ### Occlusion detection @@ -170,8 +181,6 @@ entity IntersectionStopLines { @enduml ``` -![data structure](./docs/data-structure.drawio.svg) - ### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg index fb512902ef856..ec5878a0d828e 100644 --- a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -5,31 +5,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2159px" - height="1028px" - viewBox="-0.5 -0.5 2159 1028" - content="<mxfile host="Electron" modified="2023-10-03T07:12:57.327Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="t-GOTeomSwFlDSuYHtEs" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2382px" + height="2070px" + viewBox="-0.5 -0.5 2382 2070" + content="<mxfile host="Electron" modified="2023-11-12T05:07:41.048Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="NhhSyCkZKf7kn4cJ36_Z" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - + - - - - - + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + + - - - - + - - - + + + - - - - + + + + - - + + - - + + - - + + - - + + - - + + - - + + - - - - - - - - - - - + transform="translate(292.95,0)scale(-1,1)translate(-292.95,0)rotate(90,292.95,751.55)" + pointer-events="none" + /> + + + + + + + + + + +
@@ -294,18 +285,18 @@
- closest_idx + closest_idx - - - + + +
@@ -316,21 +307,21 @@
- stuck_stop_line + stuck_stop_line - - - - - - + + + + + +
@@ -341,18 +332,18 @@
- default_stop_line + default_stop_line - - - + + +
@@ -363,18 +354,18 @@
- first_attention_stop_line + first_attention_stop_line - - - + + +
@@ -389,25 +380,25 @@
- occlusion_peeking... + occlusion_peeking... - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + + + - - - + - - - + + + - - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - +
@@ -649,18 +632,18 @@
- next + next - - - + + +
@@ -673,18 +656,18 @@
- prev + prev - - - + + +
@@ -697,18 +680,18 @@
- ego_or_entry2exit + ego_or_entry2exit - - - + + +
@@ -721,15 +704,15 @@
- entry2ego + entry2ego - - + + -
+
@@ -739,7 +722,7 @@
- IntersectionStopLines + IntersectionStopLines @@ -747,7 +730,7 @@
@@ -758,9 +741,325 @@
- PathLanelets + PathLanelets + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 5c5f5eac16b81..5de74aeb82d86 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -193,6 +193,7 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection + // TODO(Mamoru Sobue): remove `enable_occlusion_detection` variable const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c3b429680ab91..476238f6b1e9e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -936,6 +936,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( @@ -969,15 +970,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { return IntersectionModule::Indecisive{"conflicting area is empty"}; } - const auto first_conflicting_area = first_conflicting_area_opt.value(); + const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); + const auto & first_conflicting_area = first_conflicting_area_opt.value(); // generate all stop line candidates // see the doc for struct IntersectionStopLines @@ -986,22 +989,24 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( /// conflicting lanes const auto & dummy_first_attention_area = first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; - const double peeking_offset = - has_traffic_light_ ? planner_param_.occlusion.peeking_offset - : planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance; + const auto & dummy_first_attention_lane_centerline = + intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value().centerline2d() + : first_conflicting_lane.centerline2d(); const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, - planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - planner_param_.common.max_accel, planner_param_.common.max_jerk, - planner_param_.common.delay_response_time, peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, + planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, + planner_param_.common.stop_line_margin, planner_param_.common.max_accel, + planner_param_.common.max_jerk, planner_param_.common.delay_response_time, + planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, - first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = - intersection_stop_lines; + first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, + default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stop_lines; // see the doc for struct PathLanelets const auto & conflicting_area = intersection_lanelets.conflicting_area(); @@ -1083,16 +1088,91 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!default_stop_line_idx_opt) { return IntersectionModule::Indecisive{"default stop line is null"}; } + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection + if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { + return IntersectionModule::Indecisive{"occlusion stop line is null"}; + } const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + const bool is_over_default_stop_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.occlusion_attention_area = occlusion_attention_area; + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // check occlusion on detection lane + if (!occlusion_attention_divisions_) { + occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_lanelets, routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution, + planner_param_.occlusion.attention_lane_crop_curvature_threshold, + planner_param_.occlusion.attention_lane_curvature_calculation_ds); + } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); + + // get intersection area + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + // filter objects + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + + const double occlusion_dist_thr = std::fabs( + std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / + (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + auto occlusion_status = + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) + ? getOcclusionStatus( + *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + target_objects, current_pose, occlusion_dist_thr) + : OcclusionType::NOT_OCCLUDED; + occlusion_stop_state_machine_.setStateWithMarginTime( + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); + // distinguish if ego detected occlusion or RTC detects occlusion + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } // TODO(Mamoru Sobue): this part needs more formal handling - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const size_t pass_judge_line_idx = [=]() { + if (enable_occlusion_detection_) { + // if occlusion detection is enabled, pass_judge position is beyond the boundary of first + // attention area + if (has_traffic_light_) { + return occlusion_stop_line_idx; + } else if (is_occlusion_state) { + // if there is no traffic light and occlusion is detected, pass_judge position is beyond + // the boundary of first attention area + return occlusion_wo_tl_pass_judge_line_idx; + } else { + // if there is no traffic light and occlusion is not detected, pass_judge position is + // default + return default_pass_judge_line_idx; + } + } + return default_pass_judge_line_idx; + }(); debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const bool is_over_default_stop_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); const double vel_norm = std::hypot( planner_data_->current_velocity->twist.linear.x, planner_data_->current_velocity->twist.linear.y); @@ -1113,28 +1193,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection - if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { - return IntersectionModule::Indecisive{"occlusion stop line is null"}; - } - const auto collision_stop_line_idx = - is_over_default_stop_line ? closest_idx : default_stop_line_idx; - const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); - const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.occlusion_attention_area = occlusion_attention_area; - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); - - // get intersection area - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may // be detected. check if ego vehicle entered assigned lanelet @@ -1196,43 +1254,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } - // check occlusion on detection lane - if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( - occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); - } - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); - - const double occlusion_dist_thr = std::fabs( - std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / - (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) - ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, occlusion_dist_thr) - : OcclusionType::NOT_OCCLUDED; - occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); - const bool is_occlusion_cleared_with_margin = - (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion - const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; - } - const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { - occlusion_status = prev_occlusion_status_; - } else { - prev_occlusion_status_ = occlusion_status; - } - // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1257,14 +1278,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_state_machine_) : false; if (!has_traffic_light_) { - if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) { + if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { return IntersectionModule::Indecisive{ "already passed maximum peeking line in the absence of traffic light"}; } return IntersectionModule::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, has_collision_with_margin, temporal_stop_before_attention_required, closest_idx, - first_attention_stop_line_idx, occlusion_stop_line_idx}; + first_attention_stop_line_idx, occlusion_wo_tl_pass_judge_line_idx}; } // following remaining block is "has_traffic_light_" // if ego is stuck by static occlusion in the presence of traffic light, start timeout count diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 698242da67528..f1b516d72726f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -96,7 +96,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } auto & intersection_lanelets = intersection_lanelets_.value(); const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update(false, interpolated_path_info, local_footprint); + intersection_lanelets.update( + false, interpolated_path_info, local_footprint, + planner_data_->vehicle_info_.max_longitudinal_offset_m); const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e6e6fa0a1da9a..093f90e4b3df7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -215,13 +215,15 @@ static std::optional getStopLineIndexFromMap( static std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); - for (auto i = lane_start; i <= lane_end; ++i) { + for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector( footprint, tier4_autoware_utils::pose2transform(base_pose)); @@ -237,12 +239,15 @@ static std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - for (size_t i = lane_start; i <= lane_end; ++i) { + for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); @@ -259,7 +264,8 @@ getFirstPointInsidePolygonsByFootprint( std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_detection_area, + const lanelet::CompoundPolygon3d & first_attention_area, + const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, const double stop_line_margin, const double max_accel, const double max_jerk, @@ -269,31 +275,39 @@ std::optional generateIntersectionStopLines( const auto & path_ip = interpolated_path_info.path; const double ds = interpolated_path_info.ds; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / ds); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - /* - // find the index of the first point that intersects with detection_areas - const auto first_inside_detection_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); - // if path is not intersecting with detection_area, error - if (!first_inside_detection_idx_ip_opt) { - return std::nullopt; - } - const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); - */ // find the index of the first point whose vehicle footprint on it intersects with detection_area const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); std::optional first_footprint_inside_detection_ip_opt = getFirstPointInsidePolygonByFootprint( - first_detection_area, interpolated_path_info, local_footprint); + first_attention_area, interpolated_path_info, local_footprint, baselink2front); if (!first_footprint_inside_detection_ip_opt) { return std::nullopt; } const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + // (1) default stop line position on interpolated path bool default_stop_line_valid = true; int stop_idx_ip_int = -1; @@ -318,24 +332,25 @@ std::optional generateIntersectionStopLines( // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the detection area, invalid { - // NOTE: if footprints[0] is already inside the detection area, invalid const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; + occlusion_peeking_line_ip_int = + first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); } - const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); - const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; - occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); + const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto first_attention_stop_line_ip = first_footprint_inside_detection_ip; + const bool first_attention_stop_line_valid = true; // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; @@ -346,6 +361,9 @@ std::optional generateIntersectionStopLines( static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // TODO(Mamoru Sobue): maybe braking dist should be considered + const auto occlusion_wo_tl_pass_judge_line_ip = + static_cast(first_footprint_attention_centerline_ip); // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; @@ -354,7 +372,7 @@ std::optional generateIntersectionStopLines( // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint); + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; @@ -378,6 +396,7 @@ std::optional generateIntersectionStopLines( size_t first_attention_stop_line{0}; size_t occlusion_peeking_stop_line{0}; size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; }; IntersectionStopLinesTemp intersection_stop_lines_temp; @@ -388,7 +407,8 @@ std::optional generateIntersectionStopLines( {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line}, {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, - }; + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line}}; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); for (const auto & [stop_idx_ip, stop_idx] : stop_lines) { @@ -407,12 +427,6 @@ std::optional generateIntersectionStopLines( intersection_stop_lines_temp.occlusion_peeking_stop_line = intersection_stop_lines_temp.default_stop_line; } - if ( - intersection_stop_lines_temp.occlusion_peeking_stop_line > - intersection_stop_lines_temp.pass_judge_line) { - intersection_stop_lines_temp.pass_judge_line = - intersection_stop_lines_temp.occlusion_peeking_stop_line; - } IntersectionStopLines intersection_stop_lines; intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; @@ -431,6 +445,8 @@ std::optional generateIntersectionStopLines( intersection_stop_lines_temp.occlusion_peeking_stop_line; } intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; + intersection_stop_lines.occlusion_wo_tl_pass_judge_line = + intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stop_lines; } @@ -1457,13 +1473,13 @@ double calcDistanceUntilIntersectionLanelet( void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path if (!first_conflicting_area_) { - auto first = - getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_conflicting_lane_ = conflicting_.at(first.value().second); first_conflicting_area_ = conflicting_area_.at(first.value().second); @@ -1471,7 +1487,7 @@ void IntersectionLanelets::update( } if (!first_attention_area_) { auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint); + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_attention_lane_ = attention_non_preceding_.at(first.value().second); first_attention_area_ = attention_non_preceding_area_.at(first.value().second); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 2f204f06aac7c..0ff35ab5c0c23 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -69,7 +69,8 @@ std::optional generateStuckStopLine( std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_detection_area, + const lanelet::CompoundPolygon3d & first_attention_area, + const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, const double stop_line_margin, const double max_accel, const double max_jerk, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 73e60aea6471a..d1ee4c2f79410 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -74,7 +74,7 @@ struct IntersectionLanelets public: void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint); + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_; @@ -160,6 +160,7 @@ struct IntersectionStopLines std::optional occlusion_peeking_stop_line{std::nullopt}; // if the value is calculated negative, its value is 0 size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; }; struct PathLanelets From d749a7d9d5d3269ee8b5978cc74cb94de1fb8503 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Mon, 13 Nov 2023 12:48:27 +0900 Subject: [PATCH 10/18] fix: scan ground filter document (#5552) Signed-off-by: Shunsuke Miura --- .../docs/scan-ground-filter.md | 39 ++++++++++--------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index bc44544fa298c..21df6fa6ea1b9 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -30,25 +30,26 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref #### Core Parameters ![scan_ground_parameter](./image/scan_ground_filter_parameters.drawio.svg) -| Name | Type | Default Value | Description | -| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `input_frame` | string | "base_link" | frame id of input pointcloud | -| `output_frame` | string | "base_link" | frame id of output pointcloud | -| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg].
A large threshold may reduce false positive of high slope road classification but it may lead to increase false negative of non-ground classification, particularly for small objects. | -| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] when comparing with adjacent point.
A small value enhance accuracy classification of object with inclined surface. This should be considered together with `split_points_distance_tolerance` value. | -| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | -| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to distinguish far and near [m] | -| `split_height_distance` | double | 0.2 | The height threshold to distinguish ground and non-ground pointcloud when comparing with adjacent points [m].
A small threshold improves classification of non-ground point, especially for high elevation resolution pointcloud lidar. However, it might cause false positive for small step-like road surface or misaligned multiple lidar configuration. | -| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | -| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | -| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | -| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m] as `split_height_distance` and applied only for elevation_grid_mode | -| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | -| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode.
A large value enhances the prediction stability for ground surface. suitable for rough surface or multiple lidar configuration. | -| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | -| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | -| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | -| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | + +| Name | Type | Default Value | Description | +| --------------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `input_frame` | string | "base_link" | frame id of input pointcloud | +| `output_frame` | string | "base_link" | frame id of output pointcloud | +| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg].
A large threshold may reduce false positive of high slope road classification but it may lead to increase false negative of non-ground classification, particularly for small objects. | +| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] when comparing with adjacent point.
A small value enhance accuracy classification of object with inclined surface. This should be considered together with `split_points_distance_tolerance` value. | +| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | +| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to distinguish far and near [m] | +| `split_height_distance` | double | 0.2 | The height threshold to distinguish ground and non-ground pointcloud when comparing with adjacent points [m].
A small threshold improves classification of non-ground point, especially for high elevation resolution pointcloud lidar. However, it might cause false positive for small step-like road surface or misaligned multiple lidar configuration. | +| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | +| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | +| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | +| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m] as `split_height_distance` and applied only for elevation_grid_mode | +| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | +| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode.
A large value enhances the prediction stability for ground surface. suitable for rough surface or multiple lidar configuration. | +| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | +| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | +| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | +| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | ## Assumptions / Known limits From 2a697c2aadc40bd5d9d6d4f45c1b087bc529639a Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 13 Nov 2023 13:14:55 +0900 Subject: [PATCH 11/18] feat(geo_pose_projector): use geo_pose_projector in eagleye (#5513) * feat(tier4_geo_pose_projector): use tier4_geo_pose_projector in eagleye Signed-off-by: kminoda * style(pre-commit): autofix * fix(eagleye): split fix2pose Signed-off-by: kminoda * style(pre-commit): autofix * fix name: fuser -> fusion Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * style(pre-commit): autofix * update readme Signed-off-by: kminoda * style(pre-commit): autofix * add #include Signed-off-by: kminoda * add rclcpp in dependency Signed-off-by: kminoda * style(pre-commit): autofix * add limitation in readme Signed-off-by: kminoda * style(pre-commit): autofix * update tier4_localization_launch Signed-off-by: kminoda * update tier4_localization_launch Signed-off-by: kminoda * rename package Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../eagleye/eagleye_rt.launch.xml | 10 +- .../eagleye/geo_pose_converter.launch.xml | 23 ---- launch/tier4_localization_launch/package.xml | 2 +- .../geo_pose_projector/CMakeLists.txt | 17 +++ localization/geo_pose_projector/README.md | 28 +++++ .../config/geo_pose_projector.param.yaml | 5 + .../launch/geo_pose_projector.launch.xml | 13 +++ localization/geo_pose_projector/package.xml | 30 +++++ .../schema/geo_pose_projector.schema.json | 43 ++++++++ .../src/geo_pose_projector.cpp | 103 ++++++++++++++++++ .../src/geo_pose_projector.hpp | 58 ++++++++++ .../src/geo_pose_projector_node.cpp | 29 +++++ 12 files changed, 334 insertions(+), 27 deletions(-) delete mode 100644 launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml create mode 100644 localization/geo_pose_projector/CMakeLists.txt create mode 100644 localization/geo_pose_projector/README.md create mode 100644 localization/geo_pose_projector/config/geo_pose_projector.param.yaml create mode 100644 localization/geo_pose_projector/launch/geo_pose_projector.launch.xml create mode 100644 localization/geo_pose_projector/package.xml create mode 100644 localization/geo_pose_projector/schema/geo_pose_projector.schema.json create mode 100644 localization/geo_pose_projector/src/geo_pose_projector.cpp create mode 100644 localization/geo_pose_projector/src/geo_pose_projector.hpp create mode 100644 localization/geo_pose_projector/src/geo_pose_projector_node.cpp diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml index 12d84f374f7c7..6cf8d390f919b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml @@ -145,9 +145,13 @@ - - - + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml deleted file mode 100644 index d4f82e72a297b..0000000000000 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 9a137032e1277..1126914d58c5a 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -16,11 +16,11 @@ ar_tag_based_localizer automatic_pose_initializer - eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer + geo_pose_projector gyro_odometer ndt_scan_matcher pointcloud_preprocessor diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt new file mode 100644 index 0000000000000..6e2cdf32fb6c5 --- /dev/null +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(geo_pose_projector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(geo_pose_projector + src/geo_pose_projector_node.cpp + src/geo_pose_projector.cpp +) +ament_target_dependencies(geo_pose_projector) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/geo_pose_projector/README.md b/localization/geo_pose_projector/README.md new file mode 100644 index 0000000000000..a3363336da1a7 --- /dev/null +++ b/localization/geo_pose_projector/README.md @@ -0,0 +1,28 @@ +# geo_pose_projector + +## Overview + +This node is a simple node that subscribes to the geo-referenced pose topic and publishes the pose in the map frame. + +## Subscribed Topics + +| Name | Type | Description | +| ------------------------- | ---------------------------------------------------- | ------------------- | +| `input_geo_pose` | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | geo-referenced pose | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo` | map projector info | + +## Published Topics + +| Name | Type | Description | +| ------------- | ----------------------------------------------- | ------------------------------------- | +| `output_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | pose in map frame | +| `/tf` | `tf2_msgs::msg::TFMessage` | tf from parent link to the child link | + +## Parameters + +{{ json_to_markdown("localization/geo_pose_projector/schema/geo_pose_projector.schema.json") }} + +## Limitations + +The covariance conversion may be incorrect depending on the projection type you are using. The covariance of input topic is expressed in (Latitude, Longitude, Altitude) as a diagonal matrix. +Currently, we assume that the x axis is the east direction and the y axis is the north direction. Thus, the conversion may be incorrect when this assumption breaks, especially when the covariance of latitude and longitude is different. diff --git a/localization/geo_pose_projector/config/geo_pose_projector.param.yaml b/localization/geo_pose_projector/config/geo_pose_projector.param.yaml new file mode 100644 index 0000000000000..704d9dce8a4ee --- /dev/null +++ b/localization/geo_pose_projector/config/geo_pose_projector.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + publish_tf: true + parent_frame: "map" + child_frame: "pose_estimator_base_link" diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml new file mode 100644 index 0000000000000..d840add1ed1c7 --- /dev/null +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml new file mode 100644 index 0000000000000..d424ff14b9c99 --- /dev/null +++ b/localization/geo_pose_projector/package.xml @@ -0,0 +1,30 @@ + + + + geo_pose_projector + 0.1.0 + The geo_pose_projector package + Yamato Ando + Koji Minoda + Apache License 2.0 + Koji Minoda + + ament_cmake_auto + autoware_cmake + + component_interface_specs + component_interface_utils + geographic_msgs + geography_utils + geometry_msgs + rclcpp + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/geo_pose_projector/schema/geo_pose_projector.schema.json b/localization/geo_pose_projector/schema/geo_pose_projector.schema.json new file mode 100644 index 0000000000000..9daafb27f4320 --- /dev/null +++ b/localization/geo_pose_projector/schema/geo_pose_projector.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for geo_pose_projector", + "type": "object", + "definitions": { + "geo_pose_projector": { + "type": "object", + "properties": { + "publish_tf": { + "type": "boolean", + "description": "whether to publish tf", + "default": true + }, + "parent_frame": { + "type": "string", + "description": "parent frame for published tf", + "default": "map" + }, + "child_frame": { + "type": "string", + "description": "child frame for published tf", + "default": "pose_estimator_base_link" + } + }, + "required": ["publish_tf", "parent_frame", "child_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/geo_pose_projector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp new file mode 100644 index 0000000000000..6d5308b929b25 --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -0,0 +1,103 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "geo_pose_projector.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +GeoPoseProjector::GeoPoseProjector() +: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +{ + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; }); + + // Subscribe to geo_pose topic + geo_pose_sub_ = create_subscription( + "input_geo_pose", 10, [this](const GeoPoseWithCovariance::SharedPtr msg) { on_geo_pose(msg); }); + + // Publish pose topic + pose_pub_ = create_publisher("output_pose", 10); + + // Publish tf + if (publish_tf_) { + tf_broadcaster_ = std::make_unique(this); + parent_frame_ = declare_parameter("parent_frame"); + child_frame_ = declare_parameter("child_frame"); + } +} + +void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg) +{ + if (!projector_info_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000 /* ms */, "map_projector_info is not received yet."); + return; + } + + // get position + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = msg->pose.pose.position.latitude; + gps_point.longitude = msg->pose.pose.position.longitude; + gps_point.altitude = msg->pose.pose.position.altitude; + geometry_msgs::msg::Point position = + geography_utils::project_forward(gps_point, projector_info_.value()); + position.z = geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + projector_info_.value().vertical_datum); + + // Convert geo_pose to pose + PoseWithCovariance projected_pose; + projected_pose.header = msg->header; + projected_pose.pose.pose.position = position; + projected_pose.pose.pose.orientation = msg->pose.pose.orientation; + projected_pose.pose.covariance = msg->pose.covariance; + + // Covariance in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate. + // TODO(TIER IV): This swap may be invalid when using other projector type. + projected_pose.pose.covariance[0] = msg->pose.covariance[7]; + projected_pose.pose.covariance[7] = msg->pose.covariance[0]; + + pose_pub_->publish(projected_pose); + + // Publish tf + if (publish_tf_) { + tf2::Transform transform; + transform.setOrigin(tf2::Vector3( + projected_pose.pose.pose.position.x, projected_pose.pose.pose.position.y, + projected_pose.pose.pose.position.z)); + const auto localization_quat = tf2::Quaternion( + projected_pose.pose.pose.orientation.x, projected_pose.pose.pose.orientation.y, + projected_pose.pose.pose.orientation.z, projected_pose.pose.pose.orientation.w); + transform.setRotation(localization_quat); + + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header = msg->header; + transform_stamped.header.frame_id = parent_frame_; + transform_stamped.child_frame_id = child_frame_; + transform_stamped.transform = tf2::toMsg(transform); + tf_broadcaster_->sendTransform(transform_stamped); + } +} diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp new file mode 100644 index 0000000000000..b24b976b1eb61 --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEO_POSE_PROJECTOR_HPP_ +#define GEO_POSE_PROJECTOR_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +class GeoPoseProjector : public rclcpp::Node +{ +private: + using GeoPoseWithCovariance = geographic_msgs::msg::GeoPoseWithCovarianceStamped; + using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped; + using MapProjectorInfo = map_interface::MapProjectorInfo; + +public: + GeoPoseProjector(); + +private: + void on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg); + + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; + rclcpp::Subscription::SharedPtr geo_pose_sub_; + rclcpp::Publisher::SharedPtr pose_pub_; + + std::unique_ptr tf_broadcaster_; + + std::optional projector_info_ = std::nullopt; + + const bool publish_tf_; + + std::string parent_frame_; + std::string child_frame_; +}; + +#endif // GEO_POSE_PROJECTOR_HPP_ diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp new file mode 100644 index 0000000000000..97367d6b9f774 --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp @@ -0,0 +1,29 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "geo_pose_projector.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} From a37b4f36b9ecc4f59ac4b9d298f4ffaf263107b1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 13 Nov 2023 13:23:03 +0900 Subject: [PATCH 12/18] feat(simple_planning_simulator): add acceleration and steer command scaling factor for debug (#5534) * feat(simple_planning_simulator): add acceleration and steer command scaling factor Signed-off-by: kosuke55 * update params as debug Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 28 ++++++++++--------- .../sim_model_delay_steer_acc.hpp | 21 ++++++++------ .../sim_model_delay_steer_acc_geared.hpp | 21 ++++++++------ .../simple_planning_simulator_core.cpp | 8 ++++-- .../sim_model_delay_steer_acc.cpp | 13 ++++++--- .../sim_model_delay_steer_acc_geared.cpp | 14 ++++++---- 6 files changed, 65 insertions(+), 40 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 75f77489daada..0c5441c1ad9c8 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -67,19 +67,21 @@ The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves base The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------ | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | +| :------------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 289c607a18d90..92129541ff891 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -40,11 +40,14 @@ class SimModelDelaySteerAcc : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] debug_acc_scaling_factor scaling factor for accel command + * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -74,13 +77,15 @@ class SimModelDelaySteerAcc : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - const double acc_time_constant_; //!< @brief time constant for accel dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for steering dynamics - const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + const double acc_time_constant_; //!< @brief time constant for accel dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command + const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 3bda878f8cd76..376ee55f1aa5e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -40,11 +40,14 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics * @param [in] steer_dead_band dead band for steering angle [rad] + * @param [in] debug_acc_scaling_factor scaling factor for accel command + * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor); /** * @brief default destructor @@ -74,13 +77,15 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - const double acc_time_constant_; //!< @brief time constant for accel dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for steering dynamics - const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + const double acc_time_constant_; //!< @brief time constant for accel dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command + const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index f2629a0586045..07a5d34f63c39 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -230,6 +230,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double steer_time_delay = declare_parameter("steer_time_delay", 0.24); const double steer_time_constant = declare_parameter("steer_time_constant", 0.27); const double steer_dead_band = declare_parameter("steer_dead_band", 0.0); + const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0); + const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; @@ -251,12 +253,14 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + debug_acc_scaling_factor, debug_steer_scaling_factor); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 964157cdeb64c..670671165de60 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -19,7 +19,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +32,9 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) + steer_dead_band_(steer_dead_band), + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -104,8 +107,10 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); - const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 76669c9490fc6..d72b8bf116d77 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -21,7 +21,8 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) + double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, + double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,8 +34,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) - + steer_dead_band_(steer_dead_band), + debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), + debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { initializeInputQueue(dt); } @@ -113,8 +115,10 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); - const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + const double acc_des = + sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; + const double steer_des = + sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; const double steer_diff = steer - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { From a0cd7ad4bdf6449776898a8ef221921a942ca2c3 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 13 Nov 2023 17:44:07 +0900 Subject: [PATCH 13/18] fix(ndt_scan_matcher): make regularization process thread safe (#5550) * use mutex rather than main_callback_group Signed-off-by: Kento Yabuuchi * subscribe reg_pose only when regularization is enabled Signed-off-by: Kento Yabuuchi * add some comments to describe Signed-off-by: Kento Yabuuchi * use initial_pose_callback_group Signed-off-by: Kento Yabuuchi * fix typo (pauses->poses) Signed-off-by: Kento Yabuuchi Co-authored-by: Yamato Ando --------- Signed-off-by: Kento Yabuuchi Co-authored-by: Yamato Ando --- .../ndt_scan_matcher_core.hpp | 5 +- .../src/ndt_scan_matcher_core.cpp | 51 ++++++++++++++----- 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index f0d90fdfa0e08..6f9420f5bc71a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -204,9 +204,10 @@ class NDTScanMatcher : public rclcpp::Node std::mutex initial_pose_array_mtx_; // variables for regularization - const bool regularization_enabled_; + const bool regularization_enabled_; // whether to use longitudinal regularization std::deque - regularization_pose_msg_ptr_array_; + regularization_pose_msg_ptr_array_; // queue for storing regularization base poses + std::mutex regularization_mutex_; // mutex for regularization_pose_msg_ptr_array_ bool is_activated_; std::shared_ptr tf2_listener_module_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a2e89540a172f..1317b8bacf47f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -219,10 +219,20 @@ NDTScanMatcher::NDTScanMatcher() sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); - regularization_pose_sub_ = - this->create_subscription( - "regularization_pose_with_covariance", 100, - std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1)); + + // Only if regularization is enabled, subscribe to the regularization base pose + if (regularization_enabled_) { + // NOTE: The reason that the regularization subscriber does not belong to the + // main_callback_group is to ensure that the regularization callback is called even if + // sensor_callback takes long time to process. + // Both callback_initial_pose and callback_regularization_pose must not miss receiving data for + // proper interpolation. + regularization_pose_sub_ = + this->create_subscription( + "regularization_pose_with_covariance", 10, + std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1), + initial_pose_sub_opt); + } sensor_aligned_pose_pub_ = this->create_publisher("points_aligned", 10); @@ -397,7 +407,10 @@ void NDTScanMatcher::callback_initial_pose( void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { + // Get lock for regularization_pose_msg_ptr_array_ + std::lock_guard lock(regularization_mutex_); regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); + // Release lock for regularization_pose_msg_ptr_array_ } void NDTScanMatcher::callback_sensor_points( @@ -459,7 +472,9 @@ void NDTScanMatcher::callback_sensor_points( initial_pose_array_lock.unlock(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_) add_regularization_pose(sensor_ros_time); + if (regularization_enabled_) { + add_regularization_pose(sensor_ros_time); + } if (ndt_ptr_->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); @@ -816,21 +831,29 @@ std::array NDTScanMatcher::estimate_covariance( std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) { - if (regularization_pose_msg_ptr_array_.empty()) { - return std::nullopt; - } + std::shared_ptr interpolator = nullptr; + { + // Get lock for regularization_pose_msg_ptr_array_ + std::lock_guard lock(regularization_mutex_); - // synchronization - PoseArrayInterpolator interpolator(this, sensor_ros_time, regularization_pose_msg_ptr_array_); + if (regularization_pose_msg_ptr_array_.empty()) { + return std::nullopt; + } + + interpolator = std::make_shared( + this, sensor_ros_time, regularization_pose_msg_ptr_array_); - pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); + // Remove old poses to make next interpolation more efficient + pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); + + // Release lock for regularization_pose_msg_ptr_array_ + } - // if the interpolate_pose fails, 0.0 is stored in the stamp - if (rclcpp::Time(interpolator.get_current_pose().header.stamp).seconds() == 0.0) { + if (!interpolator || !interpolator->is_success()) { return std::nullopt; } - return pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + return pose_to_matrix4f(interpolator->get_current_pose().pose.pose); } void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) From 8f0767875f4428b92c890c23cce6770f00c7f838 Mon Sep 17 00:00:00 2001 From: Bupesh Rethinam Veeraiah <93628947+bupeshr@users.noreply.github.com> Date: Mon, 13 Nov 2023 17:53:46 +0900 Subject: [PATCH 14/18] test(obstacle_velocity_limiter): added test cases for particle model and approximation method (#5342) Co-authored-by: Maxime CLEMENT --- .../test/test_collision_distance.cpp | 215 +++++++++++++++++- 1 file changed, 212 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index e9cf8d9ad4dd6..c5e69e5fd8fb9 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -34,7 +34,7 @@ const auto point_in_polygon = [](const auto x, const auto y, const auto & polygo }) != polygon.outer().end(); }; -TEST(TestCollisionDistance, distanceToClosestCollision) +TEST(TestCollisionDistance, distanceToClosestCollisionParticleModel) { using obstacle_velocity_limiter::CollisionChecker; using obstacle_velocity_limiter::distanceToClosestCollision; @@ -76,11 +76,11 @@ TEST(TestCollisionDistance, distanceToClosestCollision) ASSERT_TRUE(result.has_value()); EXPECT_DOUBLE_EQ(*result, 3.0); - obstacles.points.emplace_back(2.5, -0.75); + obstacles.points.emplace_back(2.75, -0.75); result = distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); ASSERT_TRUE(result.has_value()); - EXPECT_DOUBLE_EQ(*result, 2.5); + EXPECT_DOUBLE_EQ(*result, 2.75); // Change vector and footprint vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}}; @@ -95,6 +95,7 @@ TEST(TestCollisionDistance, distanceToClosestCollision) ASSERT_FALSE(result.has_value()); obstacles.points.emplace_back(4.0, 4.0); + result = distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); ASSERT_TRUE(result.has_value()); @@ -151,6 +152,214 @@ TEST(TestCollisionDistance, distanceToClosestCollision) EXPECT_NEAR(*result, 2.121, 1e-3); } +TEST(TestCollisionDistance, distanceToClosestCollisionApproximation) +{ + using obstacle_velocity_limiter::CollisionChecker; + using obstacle_velocity_limiter::distanceToClosestCollision; + using obstacle_velocity_limiter::linestring_t; + using obstacle_velocity_limiter::polygon_t; + + obstacle_velocity_limiter::ProjectionParameters params; + params.distance_method = obstacle_velocity_limiter::ProjectionParameters::APPROXIMATION; + params.heading = 0.0; + linestring_t vector = {{0.0, 0.0}, {5.0, 0.0}}; + polygon_t footprint; + footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacle_velocity_limiter::Obstacles obstacles; + + auto EPS = 1e-2; + + std::optional result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + // Non Value obstacles + obstacles.points.emplace_back(-1.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + // inside the polygon + obstacles.points.emplace_back(4.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 4.0); + + obstacles.points.emplace_back(3.0, 0.5); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 3.04, EPS); + + obstacles.points.emplace_back(2.5, -0.75); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.61, EPS); + + obstacles.points.emplace_back(2.0, -1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.23, EPS); + + // Change vector and footprint + vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}}; + params.heading = M_PI_4; + footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacles.points.clear(); + obstacles.lines.clear(); + + // auto EPS = 1e-3; + obstacles.points.emplace_back(5.0, 1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(4.0, 4.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 5.65, EPS); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.23, EPS); + + // Change vector (opposite direction) + params.heading = -3 * M_PI_4; + vector = linestring_t{{5.0, 5.0}, {0.0, 0.0}}; + obstacles.points.clear(); + obstacles.lines.clear(); + + obstacles.points.emplace_back(1.0, 1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 5.65, EPS); + + obstacles.points.emplace_back(4.0, 3.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.23, EPS); +} + +TEST(TestCollisionDistance, distanceToClosestCollisionBicycleModel) +{ + using obstacle_velocity_limiter::CollisionChecker; + using obstacle_velocity_limiter::distanceToClosestCollision; + using obstacle_velocity_limiter::linestring_t; + using obstacle_velocity_limiter::polygon_t; + + obstacle_velocity_limiter::ProjectionParameters params; + params.model = obstacle_velocity_limiter::ProjectionParameters::BICYCLE; + params.heading = 0.0; + linestring_t vector = {{0.0, 0.0}, {5.0, 0.0}}; + polygon_t footprint; + footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacle_velocity_limiter::Obstacles obstacles; + + auto EPS = 1e-2; + + std::optional result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(-1.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(4.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(3.0, 0.5); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 3.05, EPS); + + obstacles.points.emplace_back(2.5, -0.75); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.64, EPS); + + // Change vector and footprint + vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}}; + params.heading = M_PI_2; + footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacles.points.clear(); + obstacles.lines.clear(); + + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(4.0, 4.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 6.28, EPS); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.31, EPS); + + // Change vector (opposite direction) + params.heading = -M_PI; + vector = linestring_t{{5.0, 5.0}, {0.0, 0.0}}; + obstacles.points.clear(); + obstacles.lines.clear(); + + obstacles.points.emplace_back(1.0, 1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 6.28, EPS); + + obstacles.points.emplace_back(4.0, 3.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.76, EPS); + + // change vector and footprint + params.heading = M_PI_2; + vector = linestring_t{{3.0, 3.0}, {0.0, 3.0}}; + footprint.outer() = {{1.0, -1.0}, {-4.0, 6.0}, {-5.0, -4.0}, {1.0, -4.0}}; + boost::geometry::correct(footprint); + obstacles.points.clear(); + obstacles.lines.clear(); + + obstacles.points.emplace_back(-2.0, -1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 7.34, EPS); +} + TEST(TestCollisionDistance, arcDistance) { using obstacle_velocity_limiter::arcDistance; From 88b06e0c1d70bfdd2904edfb923944422e3583b7 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 13 Nov 2023 17:58:19 +0900 Subject: [PATCH 15/18] fix(sampling_based_planner): fix runtime error (#5490) Signed-off-by: satoshi-ota --- .../include/bezier_sampler/bezier_sampling.hpp | 1 + planning/sampling_based_planner/path_sampler/src/node.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp index 624cd585b0a72..18df9dcb796d8 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index aa037755bbedd..e61f635de67c4 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -298,7 +298,8 @@ PlannerData PathSampler::createPlannerData(const Path & path) const return planner_data; } -void copyZ(const std::vector & from_traj, std::vector & to_traj) +void PathSampler::copyZ( + const std::vector & from_traj, std::vector & to_traj) { if (from_traj.empty() || to_traj.empty()) return; to_traj.front().pose.position.z = from_traj.front().pose.position.z; @@ -320,7 +321,7 @@ void copyZ(const std::vector & from_traj, std::vectorpose.position.z; } -void copyVelocity( +void PathSampler::copyVelocity( const std::vector & from_traj, std::vector & to_traj, const geometry_msgs::msg::Pose & ego_pose) { From 63220dfcb099ed7619d10f08982d5992ccc12b0a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 13 Nov 2023 19:23:36 +0900 Subject: [PATCH 16/18] docs(lane_change): stopping position when an object exists ahead (#5523) * docs(lane_change): Stopping position when an object exists ahead Signed-off-by: kosuke55 * add stuck params Signed-off-by: kosuke55 * add backward_length_buffer_for_blocking_object Signed-off-by: kosuke55 * add stop for lane change Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- ...ehavior_path_planner_lane_change_design.md | 159 +++++++++++++----- .../lane_change-stop_at_terminal.drawio.svg | 94 +++++++++++ ...hange-stop_at_terminal_no_block.drawio.svg | 76 +++++++++ ...hange-stop_far_from_target_lane.drawio.svg | 85 ++++++++++ ...ane_change-stop_not_at_terminal.drawio.svg | 70 ++++++++ ..._at_terminal_no_blocking_object.drawio.svg | 76 +++++++++ 6 files changed, 518 insertions(+), 42 deletions(-) create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg create mode 100644 planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 2e5e486afc7c3..ee74f700c4d42 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -186,6 +186,46 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +#### Stopping position when an object exists ahead + +When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. +The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. + +##### When the ego vehicle is near the end of the lane change + +Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) + +##### When the ego vehicle is not near the end of the lane change + +If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) + +##### When the target lane is far away + +When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. + +![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) + +### Lane Change When Stuck + +The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: + +- There is an obstacle in front of the current lane +- The ego vehicle is at the end of the current lane + +In this case, the safety check for lane change is relaxed compared to normal times. +Please refer to the 'stuck' section under the 'Collision checks during lane change' for more details. +The function to stop by keeping a margin against forward obstacle in the previous section is being performed to achieve this feature. + ### Lane change regulations If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. @@ -272,35 +312,36 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :------------------------------------------ | ------ | ------- | --------------------------------------------------------------------------------------------------------------- | ------------------ | -| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | -| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | -| `target_object.car` | [-] | boolean | Include car objects for safety check | true | -| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | -| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | -| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | -| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | -| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | -| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | -| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | ------ | ------- | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | +| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | ### Lane change regulations @@ -320,19 +361,53 @@ The following parameters are configurable in `lane_change.param.yaml`. The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +#### execution + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | +| `safety_check.execution.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | +| `safety_check.execution.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `safety_check.execution.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `safety_check.execution.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | +| `safety_check.execution.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +##### cancel + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.5 | +| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.5 | +| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.5 | +| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.5 | +| `safety_check.cancel.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | +| `safety_check.cancel.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.2 | +| `safety_check.cancel.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | false | +| `safety_check.cancel.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | false | +| `safety_check.cancel.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | + +##### stuck + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------ | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | +| `safety_check.stuck.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | +| `safety_check.stuck.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `safety_check.stuck.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | +| `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg new file mode 100644 index 0000000000000..f8aab60d0991d --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg new file mode 100644 index 0000000000000..0de2c5fa0a550 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg new file mode 100644 index 0000000000000..dedcc13b6ef6a --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg new file mode 100644 index 0000000000000..938b7b7d4b371 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg new file mode 100644 index 0000000000000..e07a8dcbe0354 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ lane change margin +
+
+
+ + lane change m... + + + + + + + Text is not SVG - cannot display + + + From b97ff3c595acf33169208fb185ff12e833d27cea Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 13 Nov 2023 20:17:03 +0900 Subject: [PATCH 17/18] fix(detection_by_tracker): add ignore option for each label (#5473) * fix(detection_by_tracker): add ignore for each class Signed-off-by: badai-nguyen * fix: launch Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...ra_lidar_fusion_based_detection.launch.xml | 1 + ...ar_radar_fusion_based_detection.launch.xml | 1 + .../detection/detection.launch.xml | 3 ++ .../lidar_based_detection.launch.xml | 1 + .../launch/perception.launch.xml | 2 ++ .../detection_by_tracker/CMakeLists.txt | 2 ++ .../config/detection_by_tracker.param.yaml | 11 ++++++ .../detection_by_tracker_core.hpp | 5 +-- .../include/utils/utils.hpp | 36 +++++++++++++++++++ .../launch/detection_by_tracker.launch.xml | 3 +- .../src/detection_by_tracker_core.cpp | 14 ++++++-- perception/detection_by_tracker/src/utils.cpp | 30 ++++++++++++++++ 12 files changed, 103 insertions(+), 6 deletions(-) create mode 100644 perception/detection_by_tracker/config/detection_by_tracker.param.yaml create mode 100644 perception/detection_by_tracker/include/utils/utils.hpp create mode 100644 perception/detection_by_tracker/src/utils.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 59fe3f13f1231..b3e9e9148ee60 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -195,6 +195,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 11deaad5d06cc..20b1b5a4d0b27 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -220,6 +220,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 1fcf29606891b..33994934949ac 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -73,6 +73,7 @@ + @@ -104,6 +105,7 @@ + @@ -133,6 +135,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 822c251ddad33..ac521934265d7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -61,6 +61,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 347606d91759f..8b241db9774b3 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -18,6 +18,7 @@ + @@ -174,6 +175,7 @@ + diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 236268438f852..51839027e0e41 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -27,6 +27,7 @@ include_directories( # Generate exe file set(DETECTION_BY_TRACKER_SRC src/detection_by_tracker_core.cpp + src/utils.cpp ) ament_auto_add_library(detection_by_tracker_node SHARED @@ -45,4 +46,5 @@ rclcpp_components_register_node(detection_by_tracker_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml new file mode 100644 index 0000000000000..695704050697d --- /dev/null +++ b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + tracker_ignore_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 0eacfa527750b..10affd0b94ffd 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -39,6 +39,8 @@ #include #endif +#include "utils/utils.hpp" + #include #include @@ -46,7 +48,6 @@ #include #include #include - class TrackerHandler { private: @@ -81,7 +82,7 @@ class DetectionByTracker : public rclcpp::Node std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - bool ignore_unknown_tracker_; + utils::TrackerIgnoreLabel tracker_ignore_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/utils/utils.hpp new file mode 100644 index 0000000000000..3f39125b95e03 --- /dev/null +++ b/perception/detection_by_tracker/include/utils/utils.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__UTILS_HPP_ +#define UTILS__UTILS_HPP_ + +#include + +namespace utils +{ +struct TrackerIgnoreLabel +{ + bool UNKNOWN; + bool CAR; + bool TRUCK; + bool BUS; + bool TRAILER; + bool MOTORCYCLE; + bool BICYCLE; + bool PEDESTRIAN; + bool isIgnore(const uint8_t label) const; +}; // struct TrackerIgnoreLabel +} // namespace utils + +#endif // UTILS__UTILS_HPP_ diff --git a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml index 79e1b642cc53c..95e7cbf16b388 100644 --- a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml +++ b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml @@ -3,10 +3,11 @@ - + + diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 7b436e1edd64c..2595315e1f9b2 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -160,7 +160,15 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) objects_pub_ = create_publisher( "~/output", rclcpp::QoS{1}); - ignore_unknown_tracker_ = declare_parameter("ignore_unknown_tracker", true); + // Set parameters + tracker_ignore_.UNKNOWN = declare_parameter("tracker_ignore_label.UNKNOWN"); + tracker_ignore_.CAR = declare_parameter("tracker_ignore_label.CAR"); + tracker_ignore_.TRUCK = declare_parameter("tracker_ignore_label.TRUCK"); + tracker_ignore_.BUS = declare_parameter("tracker_ignore_label.BUS"); + tracker_ignore_.TRAILER = declare_parameter("tracker_ignore_label.TRAILER"); + tracker_ignore_.MOTORCYCLE = declare_parameter("tracker_ignore_label.MOTORCYCLE"); + tracker_ignore_.BICYCLE = declare_parameter("tracker_ignore_label.BICYCLE"); + tracker_ignore_.PEDESTRIAN = declare_parameter("tracker_ignore_label.PEDESTRIAN"); // set maximum search setting for merger/divider setMaxSearchRange(); @@ -259,7 +267,7 @@ void DetectionByTracker::divideUnderSegmentedObjects( for (const auto & tracked_object : tracked_objects.objects) { const auto & label = tracked_object.classification.front().label; - if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + if (tracker_ignore_.isIgnore(label)) continue; // change search range according to label type const float max_search_range = max_search_distance_for_divider_[label]; @@ -395,7 +403,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( for (const auto & tracked_object : tracked_objects.objects) { const auto & label = tracked_object.classification.front().label; - if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + if (tracker_ignore_.isIgnore(label)) continue; // change search range according to label type const float max_search_range = max_search_distance_for_merger_[label]; diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp new file mode 100644 index 0000000000000..29a500a24cf32 --- /dev/null +++ b/perception/detection_by_tracker/src/utils.cpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils/utils.hpp" + +#include + +namespace utils +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const +{ + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); +} +} // namespace utils From 6e60821fbe8a7ca5e6bd789c9e7401ba98a66fe9 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 13 Nov 2023 20:32:22 +0900 Subject: [PATCH 18/18] feat(image_projection_based_fusion): enable to show iou value in roi_cluster_fusion debug image (#5541) * enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri * refactor iou draw settings Signed-off-by: yoshiri * add backgroud color to iou Signed-off-by: yoshiri * prevent object copying when debugger is not enabled Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../debugger.hpp | 1 + .../src/debugger.cpp | 56 ++++++++++++++++--- .../src/fusion_node.cpp | 1 + .../src/roi_cluster_fusion/node.cpp | 18 +++--- 4 files changed, 58 insertions(+), 18 deletions(-) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp index 8a6ac7672b3a8..e4b1913effed5 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp @@ -50,6 +50,7 @@ class Debugger std::vector image_rois_; std::vector obstacle_rois_; std::vector obstacle_points_; + std::vector max_iou_for_image_rois_; private: void imageCallback( diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 3f52a0de1f09d..a51c23a77aac6 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -77,6 +77,7 @@ void Debugger::clear() image_rois_.clear(); obstacle_rois_.clear(); obstacle_points_.clear(); + max_iou_for_image_rois_.clear(); } void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & stamp) @@ -84,6 +85,8 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta const boost::circular_buffer & image_buffer = image_buffers_.at(image_id); const image_transport::Publisher & image_pub = image_pubs_.at(image_id); + const bool draw_iou_score = + max_iou_for_image_rois_.size() > 0 && max_iou_for_image_rois_.size() == image_rois_.size(); for (std::size_t i = 0; i < image_buffer.size(); ++i) { if (image_buffer.at(i)->header.stamp != stamp) { @@ -91,22 +94,59 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta } auto cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding); - + // draw obstacle points for (const auto & point : obstacle_points_) { cv::circle( cv_ptr->image, cv::Point(static_cast(point.x()), static_cast(point.y())), 2, cv::Scalar(255, 255, 255), 3, 4); } + + // draw rois + const int img_height = static_cast(image_buffer.at(i)->height); + const int img_width = static_cast(image_buffer.at(i)->width); for (const auto & roi : obstacle_rois_) { - drawRoiOnImage( - cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, - cv::Scalar(255, 0, 0)); + drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(255, 0, 0)); // blue } - // TODO(yukke42): show iou_score on image for (const auto & roi : image_rois_) { - drawRoiOnImage( - cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, - cv::Scalar(0, 0, 255)); + drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(0, 0, 255)); // red + } + + // show iou_score on image + if (draw_iou_score) { + for (auto roi_index = 0; roi_index < static_cast(image_rois_.size()); ++roi_index) { + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << max_iou_for_image_rois_.at(roi_index); + std::string iou_score = stream.str(); + + // set text position + int baseline = 3; + cv::Size textSize = cv::getTextSize(iou_score, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseline); + const int text_height = static_cast(textSize.height); + const int text_width = static_cast(textSize.width); + int x = image_rois_.at(roi_index).x_offset; + int y = image_rois_.at(roi_index).y_offset; // offset for text + if (y < 0 + text_height) + y = text_height; // if roi is on the top of image, put text on lower left of roi + if (y > img_height - text_height) + y = img_height - + text_height; // if roi is on the bottom of image, put text on upper left of roi + if (x > img_width - text_width) + x = img_width - text_width; // if roi is on the right of image, put text on left of roi + if (x < 0) x = 0; // if roi is on the left of image, put text on right of roi + + // choice color by iou score + // cv::Scalar color = max_iou_for_image_rois_.at(i) > 0.5 ? cv::Scalar(0, 255, 0) : + // cv::Scalar(0, 0, 255); + cv::Scalar color = cv::Scalar(0, 0, 255); // red + + cv::rectangle( + cv_ptr->image, cv::Point(x, y - textSize.height - baseline), + cv::Point(x + textSize.width, y), cv::Scalar(255, 255, 255), + cv::FILLED); // white background + cv::putText( + cv_ptr->image, iou_score, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1, + cv::LINE_AA); // text + } } image_pub.publish(cv_ptr->toImageMsg()); diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index a540688f7e751..df797369208fe 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -203,6 +203,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec; // if matching rois exist, fuseOnSingle + // please ask maintainers before parallelize this loop because debugger is not thread safe for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 4a46c8aace696..9da9df7eff6ca 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -84,10 +84,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { - std::vector debug_image_rois; - std::vector debug_pointcloud_rois; - std::vector debug_image_points; - Eigen::Matrix4d projection; projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), @@ -100,6 +96,9 @@ void RoiClusterFusionNode::fuseOnSingleImage( tf_buffer_, /*target*/ camera_info.header.frame_id, /*source*/ input_cluster_msg.header.frame_id, camera_info.header.stamp); if (!transform_stamped_optional) { + RCLCPP_WARN_STREAM( + get_logger(), "Failed to get transform from " << input_cluster_msg.header.frame_id << " to " + << camera_info.header.frame_id); return; } transform_stamped = transform_stamped_optional.value(); @@ -151,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( max_x = std::max(static_cast(normalized_projected_point.x()), max_x); max_y = std::max(static_cast(normalized_projected_point.y()), max_y); projected_points.push_back(normalized_projected_point); - debug_image_points.push_back(normalized_projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point); } } if (projected_points.empty()) { @@ -165,7 +164,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( roi.width = max_x - min_x; roi.height = max_y - min_y; m_cluster_roi.insert(std::make_pair(i, roi)); - debug_pointcloud_rois.push_back(roi); + if (debugger_) debugger_->obstacle_rois_.push_back(roi); } for (const auto & feature_obj : input_roi_msg.feature_objects) { @@ -231,13 +230,12 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } } - debug_image_rois.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); + if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou); } + // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_rois_ = debug_pointcloud_rois; - debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); } }