From 93abafd26b201521a325c7fe6d318a0ca6710208 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 14 Jul 2023 10:10:22 +0900 Subject: [PATCH 01/26] feat(elevation_map_loader): add error handling for std::runtime_error (backport #4187) (#652) feat(elevation_map_loader): add error handling for std::runtime_error (#4187) * feat(elevation_map_loader): Add error handling for std::runtime_error * feat(elevation_map_loader): add error message output --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- .../elevation_map_loader/src/elevation_map_loader_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 7d0f0d2f7a7d3..0f30805968268 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -102,7 +102,8 @@ void ElevationMapLoaderNode::publish() try { is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag( *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); - } catch (rosbag2_storage_plugins::SqliteException & e) { + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), e.what()); is_bag_loaded = false; } if (!is_bag_loaded) { From f252dc461ebc1ace2218a3c232745036a7fca96f Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Wed, 16 Aug 2023 11:31:22 +0900 Subject: [PATCH 02/26] feat(plannig_error_monitor): update error sharp angle threshold (#681) * fix error_sharp_angle Signed-off-by: Shigekazu Fukuta * update readme Signed-off-by: Shigekazu Fukuta --------- Signed-off-by: Shigekazu Fukuta --- planning/planning_error_monitor/README.md | 2 +- .../launch/planning_error_monitor.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/planning_error_monitor/README.md b/planning/planning_error_monitor/README.md index 0890b32da6330..1f4cadb5067c4 100644 --- a/planning/planning_error_monitor/README.md +++ b/planning/planning_error_monitor/README.md @@ -47,7 +47,7 @@ This function checks if the relative angle at point1 generated from point2 and 3 | :------------------------ | :------- | :------------------------------------ | :------------ | | `error_interval` | `double` | Error Interval Distance Threshold [m] | 100.0 | | `error_curvature` | `double` | Error Curvature Threshold | 1.0 | -| `error_sharp_angle` | `double` | Error Sharp Angle Threshold | $\pi$/4 | +| `error_sharp_angle` | `double` | Error Sharp Angle Threshold | 2.0 | | `ignore_too_close_points` | `double` | Ignore Too Close Distance Threshold | 0.005 | ## Visualization diff --git a/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml b/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml index b1b2065324d80..f0f596524d2d6 100644 --- a/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml +++ b/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml @@ -9,7 +9,7 @@ - + From 66f333e2b252dd085fe966b5adf9fc317a776253 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 25 Aug 2023 17:34:28 +0900 Subject: [PATCH 03/26] fix(detection_area): search collision index only in lanelet (#695) * fix(detection_area): search collision index only in lanelet * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene_module/detection_area/scene.hpp | 5 ++++- .../scene_module/detection_area/manager.cpp | 21 +++++++++++-------- .../src/scene_module/detection_area/scene.cpp | 18 +++++++++++----- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 35b9c9d4ab55b..38f977dfc902f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface public: DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; + + int64_t lane_id_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index 8dc92269a4e1d..b508dbf353d6d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -28,22 +28,24 @@ namespace behavior_velocity_planner { namespace { -std::vector getDetectionAreaRegElemsOnPath( +using DetectionAreaWithLaneId = std::pair; + +std::vector getDetectionAreaRegElemsOnPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { - std::vector detection_area_reg_elems; + std::vector detection_areas_with_lane_id; for (const auto & p : path.points) { const auto lane_id = p.lane_ids.at(0); const auto ll = lanelet_map->laneletLayer.get(lane_id); const auto detection_areas = ll.regulatoryElementsAs(); for (const auto & detection_area : detection_areas) { - detection_area_reg_elems.push_back(detection_area); + detection_areas_with_lane_id.push_back(std::make_pair(detection_area, lane_id)); } } - return detection_area_reg_elems; + return detection_areas_with_lane_id; } std::set getDetectionAreaIdSetOnPath( @@ -52,7 +54,7 @@ std::set getDetectionAreaIdSetOnPath( { std::set detection_area_id_set; for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) { - detection_area_id_set.insert(detection_area->id()); + detection_area_id_set.insert(detection_area.first->id()); } return detection_area_id_set; } @@ -72,14 +74,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) void DetectionAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & detection_area : + for (const auto & detection_area_with_lane_id : getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = detection_area->id(); + const auto module_id = detection_area_with_lane_id.first->id(); + const auto lane_id = detection_area_with_lane_id.second; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"), - clock_)); + module_id, lane_id, *(detection_area_with_lane_id.first), planner_param_, + logger_.get_child("detection_area_module"), clock_)); } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 8d0454d2609dd..c383418c8eac8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -69,9 +69,13 @@ boost::optional getNearestCollisionPoint( } boost::optional findCollisionSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const SearchRangeIndex & search_index) { - for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto min_search_index = std::max(static_cast(0), search_index.min_idx); + const auto max_search_index = std::min(search_index.max_idx, path.points.size() - 1); + + for (size_t i = min_search_index; i < max_search_index; ++i) { const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point @@ -186,13 +190,15 @@ geometry_msgs::msg::Pose calcTargetPose( } // namespace DetectionAreaModule::DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), - planner_param_(planner_param) + planner_param_(planner_param), + lane_id_(lane_id) { } @@ -392,8 +398,10 @@ boost::optional DetectionAreaModule::createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin) const { + const auto dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_); + // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, dst_search_range); if (!collision_segment) { // No collision return {}; From 3d6ee3a27811af5ba81de20e3cc609614d41c055 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 28 Aug 2023 10:45:33 +0900 Subject: [PATCH 04/26] fix(system_monitor): extend command line to display (backport #4553) (#768) fix(system_monitor): extend command line to display (#4553) Signed-off-by: ito-san Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> --- .../process_monitor/diag_task.hpp | 129 +++++------------- .../src/process_monitor/process_monitor.cpp | 26 ++-- 2 files changed, 45 insertions(+), 110 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 183f86baa2a08..15702852a0e50 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,6 +24,25 @@ #include +/** + * @brief Struct for storing process information + */ +struct ProcessInfo +{ + std::string processId; + std::string userName; + std::string priority; + std::string niceValue; + std::string virtualImage; + std::string residentSize; + std::string sharedMemSize; + std::string processStatus; + std::string cpuUsage; + std::string memoryUsage; + std::string cpuTime; + std::string commandName; +}; + class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -46,18 +65,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", command_); - stat.add("%CPU", cpu_); - stat.add("%MEM", mem_); - stat.add("PID", pid_); - stat.add("USER", user_); - stat.add("PR", pr_); - stat.add("NI", ni_); - stat.add("VIRT", virt_); - stat.add("RES", res_); - stat.add("SHR", shr_); - stat.add("S", s_); - stat.add("TIME+", time_); + stat.add("COMMAND", info_.commandName); + stat.add("%CPU", info_.cpuUsage); + stat.add("%MEM", info_.memoryUsage); + stat.add("PID", info_.processId); + stat.add("USER", info_.userName); + stat.add("PR", info_.priority); + stat.add("NI", info_.niceValue); + stat.add("VIRT", info_.virtualImage); + stat.add("RES", info_.residentSize); + stat.add("SHR", info_.sharedMemSize); + stat.add("S", info_.processStatus); + stat.add("TIME+", info_.cpuTime); } stat.summary(level_, message_); } @@ -85,95 +104,17 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief set process id - * @param [in] pid process id - */ - void setProcessId(const std::string & pid) { pid_ = pid; } - - /** - * @brief set user name - * @param [in] user user name - */ - void setUserName(const std::string & user) { user_ = user; } - - /** - * @brief set priority - * @param [in] pr priority - */ - void setPriority(const std::string & pr) { pr_ = pr; } - - /** - * @brief set nice value - * @param [in] ni nice value - */ - void setNiceValue(const std::string & ni) { ni_ = ni; } - - /** - * @brief set virtual image - * @param [in] virt virtual image - */ - void setVirtualImage(const std::string & virt) { virt_ = virt; } - - /** - * @brief set resident size - * @param [in] res resident size + * @brief Set process information + * @param [in] info Process information */ - void setResidentSize(const std::string & res) { res_ = res; } - - /** - * @brief set shared mem size - * @param [in] shr shared mem size - */ - void setSharedMemSize(const std::string & shr) { shr_ = shr; } - - /** - * @brief set process status - * @param [in] s process status - */ - void setProcessStatus(const std::string & s) { s_ = s; } - - /** - * @brief set CPU usage - * @param [in] cpu CPU usage - */ - void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } - - /** - * @brief set memory usage - * @param [in] mem memory usage - */ - void setMemoryUsage(const std::string & mem) { mem_ = mem; } - - /** - * @brief set CPU time - * @param [in] time CPU time - */ - void setCPUTime(const std::string & time) { time_ = time; } - - /** - * @brief set Command name/line - * @param [in] command Command name/line - */ - void setCommandName(const std::string & command) { command_ = command; } + void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - - std::string pid_; //!< @brief Process Id - std::string user_; //!< @brief User Name - std::string pr_; //!< @brief Priority - std::string ni_; //!< @brief Nice value - std::string virt_; //!< @brief Virtual Image (kb) - std::string res_; //!< @brief Resident size (kb) - std::string shr_; //!< @brief Shared Mem size (kb) - std::string s_; //!< @brief Process Status - std::string cpu_; //!< @brief CPU usage - std::string mem_; //!< @brief Memory usage - std::string time_; //!< @brief CPU Time - std::string command_; //!< @brief Command name/line + struct ProcessInfo info_; //!< @brief Struct for storing process information }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f78fc00c430dc..2ce1738ecd960 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -451,27 +451,21 @@ void ProcessMonitor::getTopratedProcesses( return; } - std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - boost::trim_left(line); - boost::split(list, line, boost::is_space(), boost::token_compress_on); + std::istringstream stream(line); + + ProcessInfo info; + stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> + info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> + info.cpuUsage >> info.memoryUsage >> info.cpuTime; + + std::getline(stream, info.commandName); tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessId(list[0]); - tasks->at(index)->setUserName(list[1]); - tasks->at(index)->setPriority(list[2]); - tasks->at(index)->setNiceValue(list[3]); - tasks->at(index)->setVirtualImage(list[4]); - tasks->at(index)->setResidentSize(list[5]); - tasks->at(index)->setSharedMemSize(list[6]); - tasks->at(index)->setProcessStatus(list[7]); - tasks->at(index)->setCPUUsage(list[8]); - tasks->at(index)->setMemoryUsage(list[9]); - tasks->at(index)->setCPUTime(list[10]); - tasks->at(index)->setCommandName(list[11]); + tasks->at(index)->setProcessInformation(info); ++index; } } @@ -521,7 +515,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From 6e26a1f923e03a5528d7c1499e6671f312ea7556 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 28 Aug 2023 01:57:12 +0000 Subject: [PATCH 05/26] fix(system_monitor): disable gpu_monitor (backport #770) (#774) fix(system_monitor): disable gpu_monitor (#770) (cherry picked from commit b21f68412e20f1a8b00aa69e4f9cf1613c03719d) Co-authored-by: Hiroki OTA --- .../diagnostic_aggregator/system.param.yaml | 55 ++++++++++--------- .../launch/system_monitor.launch.py | 24 ++++---- 2 files changed, 41 insertions(+), 38 deletions(-) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index af6b9ab8a64c2..4817f396fc6f0 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -68,33 +68,34 @@ contains: [": CPU Load Average"] timeout: 3.0 - gpu: - type: diagnostic_aggregator/AnalyzerGroup - path: gpu - analyzers: - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": GPU Temperature"] - timeout: 3.0 - - usage: - type: diagnostic_aggregator/GenericAnalyzer - path: gpu_usage - contains: [": GPU Usage"] - timeout: 3.0 - - memory_usage: - type: diagnostic_aggregator/GenericAnalyzer - path: memory_usage - contains: [": GPU Memory Usage"] - timeout: 3.0 - - thermal_throttling: - type: diagnostic_aggregator/GenericAnalyzer - path: thermal_throttling - contains: [": GPU Thermal Throttling"] - timeout: 3.0 + # Disable due to NVML error + # gpu: + # type: diagnostic_aggregator/AnalyzerGroup + # path: gpu + # analyzers: + # temperature: + # type: diagnostic_aggregator/GenericAnalyzer + # path: temperature + # contains: [": GPU Temperature"] + # timeout: 3.0 + + # usage: + # type: diagnostic_aggregator/GenericAnalyzer + # path: gpu_usage + # contains: [": GPU Usage"] + # timeout: 3.0 + + # memory_usage: + # type: diagnostic_aggregator/GenericAnalyzer + # path: memory_usage + # contains: [": GPU Memory Usage"] + # timeout: 3.0 + + # thermal_throttling: + # type: diagnostic_aggregator/GenericAnalyzer + # path: thermal_throttling + # contains: [": GPU Thermal Throttling"] + # timeout: 3.0 memory: type: diagnostic_aggregator/AnalyzerGroup diff --git a/system/system_monitor/launch/system_monitor.launch.py b/system/system_monitor/launch/system_monitor.launch.py index ad4aa53beca30..41f10d02ba8c9 100644 --- a/system/system_monitor/launch/system_monitor.launch.py +++ b/system/system_monitor/launch/system_monitor.launch.py @@ -86,16 +86,18 @@ def launch_setup(context, *args, **kwargs): process_monitor_config, ], ) - with open(LaunchConfiguration("gpu_monitor_config_file").perform(context), "r") as f: - gpu_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] - gpu_monitor = ComposableNode( - package="system_monitor", - plugin="GPUMonitor", - name="gpu_monitor", - parameters=[ - gpu_monitor_config, - ], - ) + + # Disable due to NVML error + # with open(LaunchConfiguration("gpu_monitor_config_file").perform(context), "r") as f: + # gpu_monitor_config = yaml.safe_load(f)["/**"]["ros__parameters"] + # gpu_monitor = ComposableNode( + # package="system_monitor", + # plugin="GPUMonitor", + # name="gpu_monitor", + # parameters=[ + # gpu_monitor_config, + # ], + # ) # set container to run all required components in the same process container = ComposableNodeContainer( @@ -110,7 +112,7 @@ def launch_setup(context, *args, **kwargs): net_monitor, ntp_monitor, process_monitor, - gpu_monitor, + # gpu_monitor, ], output="screen", ) From c01ba627f6862291bbe354afde099e059543b7fa Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 29 Aug 2023 14:13:29 +0900 Subject: [PATCH 06/26] fix(system_monitor): high-memory process are not provided in MEM order (backport #4654) (#769) fix(system_monitor): high-memory process are not provided in MEM order (#4654) * fix(process_monitor): high-memory process are not being provided in %MEM order * changed option from 'g' to 'n' --------- Signed-off-by: ito-san Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> --- system/system_monitor/src/process_monitor/process_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 2ce1738ecd960..f8ee314e5a0ef 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -401,7 +401,7 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) bp::pipe err_pipe{err_fd[0], err_fd[1]}; bp::ipstream is_err{std::move(err_pipe)}; - bp::child c("sort -r -k 10", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); + bp::child c("sort -r -k 10 -n", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); c.wait(); if (c.exit_code() != 0) { is_err >> os.rdbuf(); From 05a607388d0ff34977ffe06f3b560834c42ca2b0 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 19 Sep 2023 12:59:03 +0900 Subject: [PATCH 07/26] feat(behavior_path_planner): resample output path (backport #1604) (#782) feat(behavior_path_planner): resample output path (#1604) * feat(behavior_path_planner): resample output path * update param Signed-off-by: Takayuki Murooka Co-authored-by: Takayuki Murooka --- .../behavior_path_planner/behavior_path_planner.param.yaml | 1 + .../config/behavior_path_planner.param.yaml | 2 ++ .../include/behavior_path_planner/parameters.hpp | 2 ++ .../src/behavior_path_planner_node.cpp | 6 +++++- 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index dd8031b0438c6..90a6432f0b83a 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -14,3 +14,4 @@ drivable_area_margin: 6.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + path_interval: 2.0 diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index e809e47b05228..f0889cc638c02 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,4 +17,6 @@ refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + path_interval: 2.0 + visualize_drivable_area_for_shared_linestrings_lanelet: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 9db58a23b70b3..31394714a9f08 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -37,6 +37,8 @@ struct BehaviorPathPlannerParameters double turn_light_on_threshold_dis_long; double turn_light_on_threshold_time; + double path_interval; + // vehicle info double wheel_base; double front_overhang; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd392853bcb23..593a6cb2cf555 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -165,6 +165,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3); p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0); p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0); + p.path_interval = declare_parameter("path_interval"); p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); @@ -535,7 +536,10 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO path->header.stamp = this->now(); RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - return path; + + const auto resampled_path = + util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); + return std::make_shared(resampled_path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 1b107a0f48649c6b59ce723a9f58ae76388284ab Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Fri, 8 Sep 2023 11:36:37 +0900 Subject: [PATCH 08/26] ci: add dispatch-push-event workflow (#803) * ci: add dispatch-push-event workflow Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * fix: change APP KEY Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: use strategy Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: change trigger Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * pre-commit fixes Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * Update .github/workflows/dispatch-push-event.yaml * Update .github/workflows/dispatch-push-event.yaml * style(pre-commit): autofix * Update .github/workflows/dispatch-push-event.yaml --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/workflows/dispatch-push-event.yaml | 45 ++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 .github/workflows/dispatch-push-event.yaml diff --git a/.github/workflows/dispatch-push-event.yaml b/.github/workflows/dispatch-push-event.yaml new file mode 100644 index 0000000000000..492083c8038f6 --- /dev/null +++ b/.github/workflows/dispatch-push-event.yaml @@ -0,0 +1,45 @@ +name: dispatch-push-event +on: + push: + +jobs: + search-dispatch-repo: + runs-on: ubuntu-latest + strategy: + matrix: + include: + - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve } + outputs: + dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }} + steps: + - name: Search dispatch repo + id: search-dispatch-repo + run: | + if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then + echo ::set-output name=value::"${{ matrix.dispatch-repo }}" + echo "Detected beta branch: ${{ github.ref_name }}" + echo "Dispatch repository: ${{ matrix.dispatch-repo }}" + fi + + dispatch-push-event: + runs-on: ubuntu-latest + needs: search-dispatch-repo + if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }} + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.INTERNAL_APP_ID }} + private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }} + + # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。 + - name: Dispatch the update-beta-branch workflow + run: | + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \ + -d '{"ref":"main"}' From 1b231f35cab6917c596a28ff4c716b11023a59d3 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 27 Sep 2023 16:36:30 +0900 Subject: [PATCH 09/26] fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf2 geometry msgs (backport #5089) (#884) fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf2 geometry msgs (#5089) Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- .../tf2_autoware_auto_msgs.hpp | 54 ------------------- 1 file changed, 54 deletions(-) diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index 7cf7081064c0b..4a29e9a0e1dec 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -38,60 +38,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { - - -/*************/ -/** Point32 **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The point to transform, as a Point32 message. - * \param t_out The transformed point, as a Point32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline -void doTransform( - const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); - t_out.x = static_cast(v_out[0]); - t_out.y = static_cast(v_out[1]); - t_out.z = static_cast(v_out[2]); -} - - -/*************/ -/** Polygon **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The polygon to transform. - * \param t_out The transformed polygon. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template<> -inline -void doTransform( - const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - // Don't call the Point32 doTransform to avoid doing this conversion every time - const auto kdl_frame = gmTransformToKDL(transform); - // We don't use std::back_inserter to allow aliasing between t_in and t_out - t_out.points.resize(t_in.points.size()); - for (size_t i = 0; i < t_in.points.size(); ++i) { - const KDL::Vector v_out = kdl_frame * KDL::Vector( - t_in.points[i].x, t_in.points[i].y, t_in.points[i].z); - t_out.points[i].x = static_cast(v_out[0]); - t_out.points[i].y = static_cast(v_out[1]); - t_out.points[i].z = static_cast(v_out[2]); - } -} - /******************/ /** Quaternion32 **/ /******************/ From 2199f69daaae9f0384bf9565ad4241bd59d22583 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 27 Sep 2023 21:56:03 +0900 Subject: [PATCH 10/26] fix(ntp_monitor): move chronyc command execution to a timer (backport #4634) (#880) fix(ntp_monitor): move chronyc command execution to a timer (#4634) * fix(ntp_monitor): move chronyc command execution to a timer * add newly added parameter timeout to config --------- Signed-off-by: ito-san Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> --- .../config/ntp_monitor.param.yaml | 1 + .../ntp_monitor/ntp_monitor.hpp | 23 +++++ .../src/ntp_monitor/ntp_monitor.cpp | 92 ++++++++++++++++--- 3 files changed, 105 insertions(+), 11 deletions(-) diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/system_monitor/config/ntp_monitor.param.yaml index db54f70d1ce59..5f46821f98aff 100644 --- a/system/system_monitor/config/ntp_monitor.param.yaml +++ b/system/system_monitor/config/ntp_monitor.param.yaml @@ -3,3 +3,4 @@ server: ntp.nict.jp offset_warn: 0.1 offset_error: 5.0 + timeout: 5 # The chronyc execution timeout will trigger a warning when this timer expires. diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index f4ee2de666c22..db264d4b7eda2 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -53,6 +53,16 @@ class NTPMonitor : public rclcpp::Node void checkOffset( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief Timer callback to execute chronyc command + */ + void onTimer(); + + /** + * @brief Timeout callback function for executing chronyc + */ + void onTimeout(); + /** * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time @@ -71,6 +81,19 @@ class NTPMonitor : public rclcpp::Node float offset_warn_; //!< @brief NTP offset(sec) to generate warning float offset_error_; //!< @brief NTP offset(sec) to generate error + int timeout_; //!< @brief Timeout duration for executing chronyc + + rclcpp::TimerBase::SharedPtr timer_; //!< @brief Timer to execute chronyc command + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group + std::mutex mutex_; //!< @brief Mutex for output from chronyc command + std::string error_str_; //!< @brief Error string + std::string pipe2_err_str_; //!< @brief Error string regarding pipe2 function call + float offset_; //!< @brief Offset value of NTP time + std::map tracking_map_; //!< @brief Output of chronyc tracking + double elapsed_ms_; //!< @brief Execution time of chronyc command + rclcpp::TimerBase::SharedPtr timeout_timer_; //!< @brief Timeout for executing chronyc + std::mutex timeout_mutex_; //!< @brief Mutex regarding timeout for executing chronyc + bool timeout_expired_; //!< @brief Timeout for executing chronyc has expired or not /** * @brief NTP offset status messages diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 10dba2f96e164..0b91b28b2b5f9 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include "system_monitor/system_monitor_utility.hpp" +#include #include #include @@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) : Node("ntp_monitor", options), updater_(this), offset_warn_(declare_parameter("offset_warn", 0.1)), - offset_error_(declare_parameter("offset_error", 5.0)) + offset_error_(declare_parameter("offset_error", 5.0)), + timeout_(declare_parameter("timeout", 5)), + timeout_expired_(false) { + using namespace std::literals::chrono_literals; + gethostname(hostname_, sizeof(hostname_)); // Check if command exists @@ -47,15 +51,15 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); -} -void NTPMonitor::update() { updater_.force_update(); } + // Start timer to execute top command + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&NTPMonitor::onTimer, this), timer_callback_group_); +} void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!chronyc_exists_) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add( @@ -67,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + double elapsed_ms; + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str = error_str_; + pipe2_err_str = pipe2_err_str_; + offset = offset_; + tracking_map = tracking_map_; + elapsed_ms = elapsed_ms_; + } + if (!pipe2_err_str.empty()) { stat.summary(DiagStatus::ERROR, "pipe2 error"); stat.add("pipe2", pipe2_err_str); @@ -91,10 +106,65 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = tracking_map.begin(); itr != tracking_map.end(); ++itr) { stat.add(itr->first, itr->second); } - stat.summary(level, offset_dict_.at(level)); - // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + // Check timeout has expired regarding executing chronyc + bool timeout_expired = false; + { + std::lock_guard lock(timeout_mutex_); + timeout_expired = timeout_expired_; + } + + if (!timeout_expired) { + stat.summary(level, offset_dict_.at(level)); + } else { + stat.summary(DiagStatus::WARN, "chronyc timeout expired"); + } + + stat.addf("execution time", "%f ms", elapsed_ms); +} + +void NTPMonitor::onTimer() +{ + // Start to measure elapsed time + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("execution_time"); + + std::string error_str; + std::string pipe2_err_str; + float offset = 0.0f; + std::map tracking_map; + + // Start timeout timer for executing chronyc + { + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = false; + } + timeout_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(timeout_), std::bind(&NTPMonitor::onTimeout, this)); + + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + + // Returning from chronyc, stop timeout timer + timeout_timer_->cancel(); + + const double elapsed_ms = stop_watch.toc("execution_time"); + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str_ = error_str; + pipe2_err_str_ = pipe2_err_str; + offset_ = offset; + tracking_map_ = tracking_map; + elapsed_ms_ = elapsed_ms; + } +} + +void NTPMonitor::onTimeout() +{ + RCLCPP_WARN(get_logger(), "Timeout occurred."); + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = true; } std::string NTPMonitor::executeChronyc( From a7a49db422e39e7d51b19c25814ef1ed5b6535ab Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Thu, 28 Sep 2023 11:21:45 +0900 Subject: [PATCH 11/26] feat(ad_service_state_monitor): change configs name (#876) change configs name Signed-off-by: asa-naki --- .../config/ad_service_state_monitor.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml index bfbb6beeaf054..c2da61aec3d0f 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml @@ -53,7 +53,7 @@ transient_local: True /perception/obstacle_segmentation/pointcloud: - module: "sensing" + module: "perception" timeout: 1.0 warn_rate: 5.0 type: "sensor_msgs/msg/PointCloud2" From 73cb81184072fca443e2838d3765dad6e2918068 Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Thu, 28 Sep 2023 11:25:45 +0900 Subject: [PATCH 12/26] feat(system_error_monitor): manual modules (#793) * feat(rviz_plugin): console meter is too large on the Rviz with FHD display, isn't it? (#587) * feat(tier4_planning/vehicle_plugin): make plugins size scalable Signed-off-by: Takamasa Horibe * remove space Signed-off-by: Takamasa Horibe * scaling Signed-off-by: Takamasa Horibe * change diag message Signed-off-by: asa-naki * fix module name Signed-off-by: asa-naki * add manual module and ignoring modules Signed-off-by: asa-naki * Revert "feat(rviz_plugin): console meter is too large on the Rviz with FHD display, isn't it? (#587)" This reverts commit f96169c31cb6183d06797df86873f2d8d66c752a. * Revert "change diag message" This reverts commit dff01ce8dd3bee99db49c6972f766d6425e153c2. * ci(pre-commit): autofix * fix spell check Signed-off-by: asa-naki * Revert "fix spell check" This reverts commit 208aa1eb880e3ab4b3dab9a1584f3193b1fb3cca. * Revert "fix module name" This reverts commit cec76535bcbc51466236493d0dcfece424f2d169. * revert ignore module Signed-off-by: asa-naki * current_mode check update Signed-off-by: asa-naki * ci(pre-commit): autofix * delete margin Signed-off-by: asa-naki --------- Signed-off-by: Takamasa Horibe Signed-off-by: asa-naki Co-authored-by: Takamasa Horibe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/system_error_monitor.param.yaml | 9 +++++++++ ...system_error_monitor.planning_simulation.param.yaml | 10 ++++++++++ .../system_error_monitor/system_error_monitor_core.hpp | 1 + .../src/system_error_monitor_core.cpp | 6 ++++++ 4 files changed, 26 insertions(+) diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 282c3b919867b..722eb943bc7d1 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -41,6 +41,15 @@ /autoware/vehicle/node_alive_monitoring: default + manual_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default + external_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 271555d1d1dc8..db71ffe5c0c90 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -41,6 +41,16 @@ /autoware/vehicle/node_alive_monitoring: default + manual_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/external_control/external_command_selector/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default + external_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index dc54b25d8e54b..a77462f9c8a30 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -56,6 +56,7 @@ struct KeyName { static constexpr const char * autonomous_driving = "autonomous_driving"; static constexpr const char * external_control = "external_control"; + static constexpr const char * manual_control = "manual_control"; }; class AutowareErrorMonitor : public rclcpp::Node diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index d6cc771e5dbd2..8db37d6d62488 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -232,6 +232,7 @@ AutowareErrorMonitor::AutowareErrorMonitor() loadRequiredModules(KeyName::autonomous_driving); loadRequiredModules(KeyName::external_control); + loadRequiredModules(KeyName::manual_control); using std::placeholders::_1; using std::placeholders::_2; @@ -460,6 +461,11 @@ void AutowareErrorMonitor::onTimer() current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO ? KeyName::autonomous_driving : KeyName::external_control; + if ( + current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO && + control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { + current_mode_ = KeyName::manual_control; + } updateHazardStatus(); publishHazardStatus(hazard_status_); From 56027b6c3b4b149782c72f68ae4ae9f6b45a960d Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Fri, 29 Sep 2023 09:50:16 +0900 Subject: [PATCH 13/26] feat(imu_corrector): add gyro_bias_validator (backport #4729) (#856) * feat(imu_corrector): add gyro_bias_validator (backport #4729) * feat(imu_corrector): add gyro_bias_validator Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * revert launch Signed-off-by: kminoda * updat Signed-off-by: kminoda * style(pre-commit): autofix * add debug publisher Signed-off-by: kminoda * minor fix Signed-off-by: kminoda * style(pre-commit): autofix * style(pre-commit): autofix * add gtest Signed-off-by: kminoda * style(pre-commit): autofix * updat e readme Signed-off-by: kminoda * style(pre-commit): autofix * add diagnostics Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * style(pre-commit): autofix * validator -> estimator Signed-off-by: kminoda * fix build Signed-off-by: kminoda * update default parameter Signed-off-by: kminoda * update comment Signed-off-by: kminoda * update readme Signed-off-by: kminoda * style(pre-commit): autofix * updated Signed-off-by: kminoda * minor update in readme Signed-off-by: kminoda * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda * update readme Signed-off-by: kminoda * style(pre-commit): autofix * Fix NG -> WARN Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: asa-naki * build(imu_corrector): add missing diagnostic_updater dependency (#4980) Signed-off-by: Esteve Fernandez * add gyro_bias estimation in diag ( #5054) Signed-off-by: asa-naki * ci(pre-commit): autofix --------- Signed-off-by: asa-naki Signed-off-by: Esteve Fernandez Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/imu_corrector/CMakeLists.txt | 25 +++- sensing/imu_corrector/README.md | 39 ++++-- .../config/gyro_bias_estimator.param.yaml | 6 + .../launch/gyro_bias_estimator.launch.xml | 16 +++ sensing/imu_corrector/package.xml | 3 + .../src/gyro_bias_estimation_module.cpp | 71 +++++++++++ .../src/gyro_bias_estimation_module.hpp | 44 +++++++ .../imu_corrector/src/gyro_bias_estimator.cpp | 115 ++++++++++++++++++ .../imu_corrector/src/gyro_bias_estimator.hpp | 66 ++++++++++ .../imu_corrector/src/imu_corrector_core.cpp | 4 +- .../imu_corrector_core.hpp | 6 +- .../test/test_gyro_bias_estimation_module.cpp | 65 ++++++++++ 12 files changed, 444 insertions(+), 16 deletions(-) create mode 100644 sensing/imu_corrector/config/gyro_bias_estimator.param.yaml create mode 100644 sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml create mode 100644 sensing/imu_corrector/src/gyro_bias_estimation_module.cpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimation_module.hpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimator.cpp create mode 100644 sensing/imu_corrector/src/gyro_bias_estimator.hpp rename sensing/imu_corrector/{include/imu_corrector => src}/imu_corrector_core.hpp (89%) create mode 100644 sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 903a6bcab07c5..79e23af53a373 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -14,19 +14,42 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +ament_auto_add_library(gyro_bias_estimation_module SHARED + src/gyro_bias_estimation_module.cpp +) + ament_auto_add_library(imu_corrector_node SHARED src/imu_corrector_core.cpp - include/imu_corrector/imu_corrector_core.hpp ) +ament_auto_add_library(gyro_bias_estimator_node SHARED + src/gyro_bias_estimator.cpp +) + +target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module) + rclcpp_components_register_node(imu_corrector_node PLUGIN "imu_corrector::ImuCorrector" EXECUTABLE imu_corrector ) +rclcpp_components_register_node(gyro_bias_estimator_node + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator +) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + add_testcase(test/test_gyro_bias_estimation_module.cpp) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index aedbcbc3c09cd..54b4f70abda5e 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -1,6 +1,6 @@ # imu_corrector -## Purpose +## imu_corrector `imu_corrector_node` is a node that correct imu data. @@ -10,8 +10,6 @@ -## Inputs / Outputs - ### Input | Name | Type | Description | @@ -24,9 +22,7 @@ | --------- | ----------------------- | ------------------ | | `~output` | `sensor_msgs::msg::Imu` | corrected imu data | -## Parameters - -### Core Parameters +### Parameters | Name | Type | Description | | ---------------------------- | ------ | ------------------------------------- | @@ -37,12 +33,33 @@ | `angular_velocity_stddev_yy` | double | pitch rate standard deviation [rad/s] | | `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] | -## Assumptions / Known limits +## gyro_bias_estimator + +`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range. -## (Optional) Error detection and handling +Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped. -## (Optional) Performance characterization +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | -## (Optional) References/External links +### Output -## (Optional) Future extensions / Unimplemented parts +| Name | Type | Description | +| -------------------- | ------------------------------------ | ----------------------------- | +| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | + +### Parameters + +| Name | Type | Description | +| --------------------------- | ------ | ----------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | +| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | +| `data_num_threshold` | int | number of data used to calculate bias | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml new file mode 100644 index 0000000000000..d5868e1df416a --- /dev/null +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.0015 # [rad/s] + velocity_threshold: 0.0 # [m/s] + timestamp_threshold: 0.1 # [s] + data_num_threshold: 200 # [num] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml new file mode 100644 index 0000000000000..a25ce5f90ed27 --- /dev/null +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 71fece2622338..5fba088d858fa 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -9,10 +9,13 @@ ament_cmake_auto + diagnostic_updater rclcpp rclcpp_components sensor_msgs + ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..2deb6088f6542 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -0,0 +1,71 @@ +// 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 "gyro_bias_estimation_module.hpp" + +namespace imu_corrector +{ +GyroBiasEstimationModule::GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold) +: velocity_threshold_(velocity_threshold), + timestamp_threshold_(timestamp_threshold), + data_num_threshold_(data_num_threshold), + is_stopped_(false) +{ +} + +void GyroBiasEstimationModule::update_gyro( + const double time, const geometry_msgs::msg::Vector3 & gyro) +{ + if (time - last_velocity_time_ > timestamp_threshold_) { + return; + } + if (!is_stopped_) { + return; + } + gyro_buffer_.push_back(gyro); + if (gyro_buffer_.size() > data_num_threshold_) { + gyro_buffer_.pop_front(); + } +} + +void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +{ + is_stopped_ = velocity <= velocity_threshold_; + last_velocity_time_ = time; +} + +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +{ + if (gyro_buffer_.size() < data_num_threshold_) { + throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + } + + geometry_msgs::msg::Vector3 bias; + bias.x = 0.0; + bias.y = 0.0; + bias.z = 0.0; + for (const auto & gyro : gyro_buffer_) { + bias.x += gyro.x; + bias.y += gyro.y; + bias.z += gyro.z; + } + bias.x /= gyro_buffer_.size(); + bias.y /= gyro_buffer_.size(); + bias.z /= gyro_buffer_.size(); + return bias; +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp new file mode 100644 index 0000000000000..6ebae942fff5d --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -0,0 +1,44 @@ +// 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 GYRO_BIAS_ESTIMATION_MODULE_HPP_ +#define GYRO_BIAS_ESTIMATION_MODULE_HPP_ + +#include + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModule +{ +public: + GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold); + geometry_msgs::msg::Vector3 get_bias() const; + void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); + void update_velocity(const double time, const double velocity); + +private: + const double velocity_threshold_; + const double timestamp_threshold_; + const size_t data_num_threshold_; + bool is_stopped_; + std::deque gyro_buffer_; + + double last_velocity_time_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp new file mode 100644 index 0000000000000..c114ba6caba5d --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -0,0 +1,115 @@ +// 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 "gyro_bias_estimator.hpp" + +namespace imu_corrector +{ +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & node_options) +: Node("gyro_bias_validator", node_options), + gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), + angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), + angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), + angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + updater_(this), + gyro_bias_(std::nullopt) +{ + updater_.setHardwareID(get_name()); + updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + + const double velocity_threshold = declare_parameter("velocity_threshold"); + const double timestamp_threshold = declare_parameter("timestamp_threshold"); + const size_t data_num_threshold = + static_cast(declare_parameter("data_num_threshold")); + gyro_bias_estimation_module_ = std::make_unique( + velocity_threshold, timestamp_threshold, data_num_threshold); + + imu_sub_ = create_subscription( + "~/input/imu_raw", rclcpp::SensorDataQoS(), + [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); + twist_sub_ = create_subscription( + "~/input/twist", rclcpp::SensorDataQoS(), + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); + + gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); +} + +void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) +{ + // Update gyro data + gyro_bias_estimation_module_->update_gyro( + rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); + + // Estimate gyro bias + try { + gyro_bias_ = gyro_bias_estimation_module_->get_bias(); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + } + + // Publish results for debugging + if (gyro_bias_ != std::nullopt) { + Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); + gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); + } + + // Update diagnostics + updater_.force_update(); +} + +void GyroBiasEstimator::callback_twist( + const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + gyro_bias_estimation_module_->update_velocity( + rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); +} + +void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (gyro_bias_ == std::nullopt) { + stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); + } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); + // Validation + const bool is_bias_small_enough = + std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + stat.add("gyro_bias", "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.add( + "gyro_bias", + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " + "imu_corrector. You may also use the output of gyro_bias_estimator."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); + } + } +} + +} // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp new file mode 100644 index 0000000000000..37ca1d81dc8fa --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -0,0 +1,66 @@ +// 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 GYRO_BIAS_ESTIMATOR_HPP_ +#define GYRO_BIAS_ESTIMATOR_HPP_ + +#include "gyro_bias_estimation_module.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimator : public rclcpp::Node +{ +private: + using Imu = sensor_msgs::msg::Imu; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + using Vector3 = geometry_msgs::msg::Vector3; + +public: + explicit GyroBiasEstimator(const rclcpp::NodeOptions & node_options); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); + void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + + rclcpp::Publisher::SharedPtr gyro_bias_pub_; + + std::unique_ptr gyro_bias_estimation_module_; + + const double gyro_bias_threshold_; + const double angular_velocity_offset_x_; + const double angular_velocity_offset_y_; + const double angular_velocity_offset_z_; + + diagnostic_updater::Updater updater_; + + std::optional gyro_bias_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index c540ab828c32c..491f720db7dd0 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "imu_corrector/imu_corrector_core.hpp" +#include "imu_corrector_core.hpp" + +#include namespace imu_corrector { diff --git a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp similarity index 89% rename from sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp rename to sensing/imu_corrector/src/imu_corrector_core.hpp index 4e4d154d92aac..30e361fe95b34 100644 --- a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -11,8 +11,8 @@ // 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 IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ -#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#ifndef IMU_CORRECTOR_CORE_HPP_ +#define IMU_CORRECTOR_CORE_HPP_ #include @@ -42,4 +42,4 @@ class ImuCorrector : public rclcpp::Node }; } // namespace imu_corrector -#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..a0da4d0e24e17 --- /dev/null +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -0,0 +1,65 @@ +// 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 "../src/gyro_bias_estimation_module.hpp" + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModuleTest : public ::testing::Test +{ +protected: + double velocity_threshold = 1.0; + double timestamp_threshold = 5.0; + size_t data_num_threshold = 10; + GyroBiasEstimationModule module = + GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); +}; + +TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); + ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); + ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) +{ + ASSERT_THROW(module.get_bias(), std::runtime_error); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_THROW(module.get_bias(), std::runtime_error); +} +} // namespace imu_corrector From bd8b4c64ae7911ab21cf452361035f222b2c5ab9 Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Fri, 29 Sep 2023 13:15:05 +0900 Subject: [PATCH 14/26] feat(system_error_monitor): add ignore_until_waiting_for_route module (#888) * add ignore_module Signed-off-by: asa-naki * add description Signed-off-by: asa-naki * ci(pre-commit): autofix * change name ignore_until_waiting_for_route Signed-off-by: asa-naki * update description Signed-off-by: asa-naki * rename function name and delete planning state Signed-off-by: asa-naki * update description Signed-off-by: asa-naki * Update --------- Signed-off-by: asa-naki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA --- .../config/system_error_monitor.param.yaml | 3 +- ...ror_monitor.planning_simulation.param.yaml | 11 ++++--- .../system_error_monitor_core.hpp | 1 + .../src/system_error_monitor_core.cpp | 32 ++++++++++++++++++- 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 722eb943bc7d1..fde96aeeebfb2 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -4,7 +4,7 @@ # lf_at: diag level where it becomes Latent Fault # spf_at: diag level where it becomes Single Point Fault # auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# +# ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route. # Note: # empty-value for sf_at, lf_at and spf_at is "none" # default values are: @@ -12,6 +12,7 @@ # lf_at: "warn" # spf_at: "error" # auto_recovery: "true" +# ignore_until_waiting_for_route: "false" --- /**: ros__parameters: diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index db71ffe5c0c90..cd968ce615e0b 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -4,7 +4,7 @@ # lf_at: diag level where it becomes Latent Fault # spf_at: diag level where it becomes Single Point Fault # auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# +# ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route. # Note: # empty-value for sf_at, lf_at and spf_at is "none" # default values are: @@ -12,15 +12,16 @@ # lf_at: "warn" # spf_at: "error" # auto_recovery: "true" +# ignore_until_waiting_for_route: "false" --- /**: ros__parameters: required_modules: autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default + /autoware/control/autonomous_driving/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"} /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"} /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -30,7 +31,7 @@ /autoware/perception/node_alive_monitoring: default - /autoware/planning/node_alive_monitoring: default + /autoware/planning/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"} /autoware/planning/performance_monitoring/trajectory_validation: default /autoware/sensing/node_alive_monitoring: default @@ -53,7 +54,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index a77462f9c8a30..f033618b2305d 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -48,6 +48,7 @@ struct DiagConfig std::string lf_at; std::string spf_at; bool auto_recovery; + bool ignore_until_waiting_for_route; }; using RequiredModules = std::vector; diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 8db37d6d62488..1a90a55c73187 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -208,6 +208,22 @@ int isInNoFaultCondition( return false; } +bool ignoreUntilWaitingForRoute( + const autoware_auto_system_msgs::msg::AutowareState & autoware_state, + const DiagConfig & required_module) +{ + using autoware_auto_system_msgs::msg::AutowareState; + using tier4_control_msgs::msg::GateMode; + + const auto is_in_autonomous_ignore_state = + (autoware_state.state == AutowareState::INITIALIZING) || + (autoware_state.state == AutowareState::WAITING_FOR_ROUTE); + + if (is_in_autonomous_ignore_state && required_module.ignore_until_waiting_for_route) { + return true; + } + return false; +} } // namespace AutowareErrorMonitor::AutowareErrorMonitor() @@ -322,11 +338,22 @@ void AutowareErrorMonitor::loadRequiredModules(const std::string & key) std::string auto_recovery_approval_str; this->get_parameter_or(auto_recovery_key, auto_recovery_approval_str, std::string("true")); + const auto ignore_until_waiting_for_route_key = + module_name_with_prefix + std::string(".ignore_until_waiting_for_route"); + std::string ignore_until_waiting_for_route_str; + this->get_parameter_or( + ignore_until_waiting_for_route_key, ignore_until_waiting_for_route_str, std::string("false")); + // Convert auto_recovery_approval_str to bool bool auto_recovery_approval{}; std::istringstream(auto_recovery_approval_str) >> std::boolalpha >> auto_recovery_approval; - required_modules.push_back({param_module, sf_at, lf_at, spf_at, auto_recovery_approval}); + bool ignore_until_waiting_for_route{}; + std::istringstream(ignore_until_waiting_for_route_str) >> std::boolalpha >> + ignore_until_waiting_for_route; + + required_modules.push_back( + {param_module, sf_at, lf_at, spf_at, auto_recovery_approval, ignore_until_waiting_for_route}); } required_modules_map_.insert(std::make_pair(key, required_modules)); @@ -493,6 +520,9 @@ uint8_t AutowareErrorMonitor::getHazardLevel( using autoware_auto_system_msgs::msg::HazardStatus; if (isOverLevel(diag_level, required_module.spf_at)) { + if (ignoreUntilWaitingForRoute(*autoware_state_, required_module)) { + return HazardStatus::NO_FAULT; + } return HazardStatus::SINGLE_POINT_FAULT; } if (isOverLevel(diag_level, required_module.lf_at)) { From 1485bf72c77e255f148bce1cf6a55bdd3cd0108f Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Fri, 6 Oct 2023 15:10:19 +0900 Subject: [PATCH 15/26] feat(imu_corrector): add gyro bias log (#918) add gyro_bias log Signed-off-by: asa-naki --- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index c114ba6caba5d..7a5bdf73600c9 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -105,6 +105,11 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " "imu_corrector. You may also use the output of gyro_bias_estimator."); stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); + RCLCPP_WARN( + get_logger(), "gyro_bias_x: %lf, gyro_bias_y: %lf, gyro_bias_z: %lf", + (gyro_bias_.value().x - angular_velocity_offset_x_), + (gyro_bias_.value().y - angular_velocity_offset_y_), + (gyro_bias_.value().z - angular_velocity_offset_z_)); } } } From c80c986e9b4b37f0d6af92c041bfc174819ef843 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 10 Oct 2023 14:08:45 +0900 Subject: [PATCH 16/26] fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909) * fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing * fix implementation * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_planner_node.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e5c1b89454f4e..073f9015d4616 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -50,6 +50,7 @@ #include #include +#include #include namespace behavior_path_planner @@ -136,7 +137,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); + std::pair getPath( + const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 593a6cb2cf555..c241ed0324353 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,7 +478,10 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto path = getPath(output); + const auto paths = getPath(output); + const auto path = paths.first; + const auto original_path = paths.second; + const auto path_candidate = getPathCandidate(output); planner_data_->prev_output_path = path; @@ -504,7 +507,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -527,7 +530,9 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) +// output: spline interpolated path, original path +std::pair BehaviorPathPlannerNode::getPath( + const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -539,7 +544,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO const auto resampled_path = util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_shared(resampled_path); + return std::make_pair(std::make_shared(resampled_path), path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From ea62ea391780729f501c3283ac6a7aa49c7be365 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Oct 2023 11:14:04 +0900 Subject: [PATCH 17/26] fix(detection_area): fix overline function (#930) * fix(detection_area): fix overline function * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/scene_module/detection_area/scene.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index c383418c8eac8..1ca450dfb1a08 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -357,8 +357,13 @@ bool DetectionAreaModule::isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return tier4_autoware_utils::calcSignedArcLength( - path.points, self_pose.position, line_pose.position) < 0; + const PointWithSearchRangeIndex src_point_with_search_range_index = + planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position); + const PointWithSearchRangeIndex dst_point_with_search_range_index = { + line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)}; + + return planning_utils::calcSignedArcLengthWithSearchIndex( + path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0; } bool DetectionAreaModule::hasEnoughBrakingDistance( From 84497a7744b1b9b2a1ff27570e36b83e6db7e7d6 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Oct 2023 13:15:43 +0900 Subject: [PATCH 18/26] fix(behavior_path): fix base points vanishing and inconsistent lane_ids on the spline interpolated path (#929) * add base points to resampled path in behavior_path * Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)" This reverts commit c80c986e9b4b37f0d6af92c041bfc174819ef843. * ci(pre-commit): autofix * fix insert * fix: not interpolate behavior velocity path * Revert "Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)"" This reverts commit e6dd540d354a9cdb488da0b12157b55c8c895ae9. --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/path_utilities.cpp | 34 +++++++++++++------ .../behavior_velocity_planner/src/node.cpp | 4 +-- 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 9a02ced1c41c4..34e5111c1a2cb 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -90,7 +90,18 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + + constexpr double epsilon = 0.01; + for (const auto & b : base_points) { + const auto is_almost_same_value = std::find_if( + sampling_points.begin(), sampling_points.end(), + [&](const auto v) { return std::abs(v - b) < epsilon; }); + if (is_almost_same_value == sampling_points.end()) { + sampling_points.push_back(b); + } + } + std::sort(sampling_points.begin(), sampling_points.end()); if (base_points.empty() || sampling_points.empty()) { return path; @@ -122,22 +133,25 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // For LaneIds, Type, Twist // - // ------|----|----|----|----|----|----|-------> resampled - // [0] [1] [2] [3] [4] [5] [6] + // ------|----|----|----|----|----|----|----|--------> resampled + // [0] [1] [2] [3] [4] [5] [6] [7] // - // --------|---------------|----------|---------> base - // [0] [1] [2] + // ------|---------------|----------|-------|---> base + // [0] [1] [2] [3] // - // resampled[0~3] = base[0] - // resampled[4~5] = base[1] - // resampled[6] = base[2] + // resampled[0] = base[0] + // resampled[1~3] = base[1] + // resampled[4~5] = base[2] + // resampled[6~7] = base[3] // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) { + while (base_idx < base_points.size() - 1 && + ((sampling_points.at(i) > base_points.at(base_idx)) || + (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { ++base_idx; } - size_t ref_idx = std::max(static_cast(base_idx) - 1, 0); + size_t ref_idx = std::max(static_cast(base_idx), 0); if (i == resampled_path.points.size() - 1) { ref_idx = base_points.size() - 1; // for last point } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ad30db2917ecd..c8600e25716b9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger( const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); // check stop point - auto output_path_msg = filterStopPathPoint(interpolated_path_msg); + auto output_path_msg = filterStopPathPoint(filtered_path); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); From 368e66f810b4fc52e78ef3df358e1f05f4a3558e Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Oct 2023 19:06:35 +0900 Subject: [PATCH 19/26] fix: add error handling when path is invalid (#934) * fix(behavior_path): delete duplicated * add error handling * fix: when path size is 1 --- .../behavior_path_planner/src/utilities.cpp | 23 ------------------- .../obstacle_avoidance_planner/node.hpp | 12 ++++++++++ .../obstacle_avoidance_planner/src/node.cpp | 3 +++ planning/obstacle_stop_planner/src/node.cpp | 4 ++++ 4 files changed, 19 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 84f384cf3b726..1ae670216938b 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -26,29 +26,6 @@ #include #include -namespace tier4_autoware_utils -{ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx) -{ - validateNonEmpty(points); - - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); - - const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; - const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; - - if (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); - } - - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); - return cross_vec(2) / segment_vec.norm(); -} -} // namespace tier4_autoware_utils - namespace drivable_area_utils { double quantize(const double val, const double resolution) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 5c1036b9782f3..f5f43312ead04 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -53,6 +53,10 @@ template boost::optional lerpPose( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return {}; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -94,6 +98,10 @@ template double lerpTwistX( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return 0.0; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -116,6 +124,10 @@ template double lerpPoseZ( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return 0.0; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index eb75fcbee9006..c5f642ebb7973 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1093,6 +1093,9 @@ void ObstacleAvoidancePlanner::calcVelocity( const std::vector & path_points, std::vector & traj_points) const { + if (path_points.size() < 2) { + return; + } for (size_t i = 0; i < traj_points.size(); i++) { const size_t nearest_seg_idx = [&]() { const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 324a37bfa9559..44de6e2cdf9cb 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -753,6 +753,10 @@ void ObstacleStopPlannerNode::insertVelocity( const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, const StopParam & stop_param) { + if (output.size() < 3) { + return; + } + if (planner_data.stop_require) { // insert stop point const auto traj_end_idx = output.size() - 1; From 3c22be06610071d2f7fccea82ba02aa5f1ea531a Mon Sep 17 00:00:00 2001 From: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> Date: Mon, 23 Oct 2023 15:17:40 +0900 Subject: [PATCH 20/26] feat(system_error_monitor): add ignore hartbeat timeout in initializing state (#972) * add ignore hartbeat timeout in initializing state Signed-off-by: asa-naki * fix typo * Update comment * ci(pre-commit): autofix * fix typo * ci(pre-commit): autofix * update comment * ci(pre-commit): autofix --------- Signed-off-by: asa-naki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/system_error_monitor_core.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1a90a55c73187..434009eb69d30 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -478,8 +478,16 @@ void AutowareErrorMonitor::onTimer() } return; } - + // Heartbeat in AutowareState,diag_array times out during AutowareState INITIALIZING due to high + // processing load,add a disable function to avoid Emergencies in isDataHeartbeatTimeout() in + // AutowareState INITIALIZING. if (isDataHeartbeatTimeout()) { + if ((autoware_state_->state == autoware_auto_system_msgs::msg::AutowareState::INITIALIZING)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "ignore heartbeat timeout in initializing state"); + return; + } updateTimeoutHazardStatus(); publishHazardStatus(hazard_status_); return; From a2cbf8cafbbaeaccdcb36a69bb9ff4a91854993a Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 22 Nov 2023 17:34:31 +0900 Subject: [PATCH 21/26] fix(route_handler): fix threshold for removing overlapping points (backport #1015) (#1016) fix(route_handler): fix threshold for removing overlapping points (#1015) * fix(route_handler): fix threshold for removing overlapping points * fix (cherry picked from commit 733bca4c12dc149365b5d00e00e80af13d16e12c) Co-authored-by: Hiroki OTA --- planning/route_handler/src/route_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ad224028ef3c2..939fc71e50758 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -121,7 +121,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) continue; } - constexpr double min_dist = 0.001; + constexpr double min_dist = 0.1; if ( tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { From 861c01e987358e325afacb6f33e6496844d77a94 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 13 Dec 2023 15:37:43 +0900 Subject: [PATCH 22/26] fix(system_monitor): fix program command line reading (backport #5191, #5430) (#995) * perf(system_monitor): fix program command line reading (#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix spelling commandline->command_line 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> * fix(system_monitor): output command line (#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Owen-Liuyuxuan Signed-off-by: takeshi.iwanari Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: takeshi-iwanari Co-authored-by: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> --- .../process_monitor/process_monitor.hpp | 8 +++++ .../src/process_monitor/process_monitor.cpp | 32 +++++++++++++++++-- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..ec2a90c6b27e4 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string & command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f8ee314e5a0ef..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,6 +414,27 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string & command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath, std::ios::in | std::ios::binary); + + if (commandFile.is_open()) { + std::vector buffer; + std::copy( + std::istream_iterator(commandFile), std::istream_iterator(), + std::back_inserter(buffer)); + commandFile.close(); + std::replace( + buffer.begin(), buffer.end(), '\0', + ' '); // 0x00 is used as delimiter in /cmdline instead of 0x20 (space) + command = std::string(buffer.begin(), buffer.end()); + return (buffer.size() > 0) ? true : false; // cmdline is empty if it is kernel process + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -462,7 +483,14 @@ void ProcessMonitor::getTopratedProcesses( info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> info.cpuUsage >> info.memoryUsage >> info.cpuTime; - std::getline(stream, info.commandName); + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); tasks->at(index)->setProcessInformation(info); @@ -515,7 +543,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From 65e903c18af4625407c1be46544574c8d1c79992 Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Tue, 19 Dec 2023 14:02:54 +0900 Subject: [PATCH 23/26] feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta * fix parameter name Signed-off-by: Shigekazu Fukuta * Adapted from PR #2647 Signed-off-by: Shigekazu Fukuta * ci(pre-commit): autofix * fix build error * ci(pre-commit): autofix * remove comment line * remove logic * Delete parameters other than those added this time * ci(pre-commit): autofix * add stop vehicle check * ci(pre-commit): autofix * fix stop velocity threshold * fix engage obstacle clear and stop threshold * ci(pre-commit): autofix --------- Signed-off-by: Shigekazu Fukuta Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/obstacle_stop_planner/README.md | 6 ++ .../config/obstacle_stop_planner.param.yaml | 2 + .../include/obstacle_stop_planner/node.hpp | 45 ++++++++- planning/obstacle_stop_planner/src/node.cpp | 99 +++++++++++++------ 4 files changed, 118 insertions(+), 34 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 95274b6a4829d..ed46c3f6bc154 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -39,6 +39,12 @@ When the deceleration section is inserted, the start point of the section is ins ## Modules +### Common Parameter + +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | + ### Obstacle Stop Planner #### Role diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index c4be8d7c35789..a0acc76b357f3 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] + stop_planner: stop_margin: 5.0 # stop margin distance from obstacle on the path [m] min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 0a4dc99493bc7..7ca2da6be2354 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include namespace motion_planning @@ -76,6 +77,8 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; +using PointCloud = pcl::PointCloud; + struct StopPoint { TrajectoryPoint point{}; @@ -91,6 +94,17 @@ struct SlowDownSection double velocity; }; +struct ObstacleWithDetectionTime +{ + explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) + : detection_time(t), point(p) + { + } + + rclcpp::Time detection_time; + pcl::PointXYZ point; +}; + class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -101,7 +115,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node bool enable_slow_down; // set True, slow down for obstacle beside the path double max_velocity; // max velocity [m/s] double lowpass_gain; // smoothing calculated current acceleration [-] - double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] + double chattering_threshold; // keep slow down or stop state if obstacle vanished [s] double max_yaw_deviation_rad; // maximum ego yaw deviation from trajectory [rad] (measures // against overlapping lanes) }; @@ -184,12 +198,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; std::shared_ptr lpf_acc_{nullptr}; - boost::optional latest_slow_down_section_{}; + boost::optional latest_slow_down_section_{boost::none}; + std::vector obstacle_history_{}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - rclcpp::Time last_detection_time_; nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; @@ -305,6 +319,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); + + void updateObstacleHistory(const rclcpp::Time & now, const double chattering_threshold) + { + for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > chattering_threshold; + + if (expired) { + itr = obstacle_history_.erase(itr); + continue; + } + + itr++; + } + } + + PointCloud::Ptr getOldPointCloudPtr() const + { + PointCloud::Ptr ret(new PointCloud); + + for (const auto & p : obstacle_history_) { + ret->push_back(p.point); + } + + return ret; + } }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 44de6e2cdf9cb..45a9757175f81 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,6 +42,8 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::getRPY; +using PointCloud = pcl::PointCloud; + namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -443,7 +445,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down", false); p.max_velocity = declare_parameter("max_velocity", 20.0); - p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); + p.chattering_threshold = declare_parameter("chattering_threshold", 0.5); p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); lpf_acc_ = std::make_shared(0.0, p.lowpass_gain); const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0); @@ -503,7 +505,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); - last_detection_time_ = this->now(); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -632,9 +633,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; - const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > - node_param_.hunting_threshold; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -662,6 +661,11 @@ void ObstacleStopPlannerNode::searchObstacle( return; } + const auto now = this->now(); + const bool is_stopping = (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 0.001); + const double history_erase_sec = (is_stopping) ? node_param_.chattering_threshold : 0.0; + updateObstacleHistory(now, history_erase_sec); + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; @@ -721,37 +725,79 @@ void ObstacleStopPlannerNode::searchObstacle( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - planner_data.found_collision_points = withinPolygon( + const auto found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; + if (found_collision_points) { + pcl::PointXYZ nearest_collision_point; + rclcpp::Time nearest_collision_point_time; getNearestPoint( - *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, - &planner_data.nearest_collision_point_time); + *collision_pointcloud_ptr, p_front, &nearest_collision_point, + &nearest_collision_point_time); - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - - planner_data.stop_require = planner_data.found_collision_points; - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, planner_data.decimate_trajectory_collision_index, - planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, - &planner_data.stop_require, &output, trajectory_header); + obstacle_history_.emplace_back(now, nearest_collision_point); break; } } } + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + std::vector one_step_move_vehicle_polygon; + // create one step polygon for vehicle + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.expand_stop_range); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, + PolygonType::Vehicle); + + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + // check new collision points + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + + planner_data.stop_require = planner_data.found_collision_points; + + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); + + if (!planner_data.stop_require) { + obstacle_history_.clear(); + } + + break; + } + } } void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const double current_vel, const StopParam & stop_param) + [[maybe_unused]] const std_msgs::msg::Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param) { if (output.size() < 3) { return; @@ -799,17 +845,8 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, current_vel); - if ( - !latest_slow_down_section_ && - dist_baselink_to_obstacle + index_with_dist_remain.get().second < - vehicle_info.max_longitudinal_offset_m) { - latest_slow_down_section_ = slow_down_section; - } - insertSlowDownSection(slow_down_section, output); } - - last_detection_time_ = trajectory_header.stamp; } if (node_param_.enable_slow_down && latest_slow_down_section_) { From 751ad2b73f14f06984ea3b63ff258fc2b602f572 Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Tue, 14 May 2024 11:15:19 +0900 Subject: [PATCH 24/26] fix(pre-commit): add flake8-ros (#1291) * update pre-commit-config * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 19 ++----------------- common/tier4_debug_tools/package.xml | 2 +- 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6b74680a99652..278ef30c4964c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,8 +35,9 @@ repos: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.6.0 + rev: v0.8.0 hooks: + - id: flake8-ros - id: prettier-xacro - id: prettier-launch-xml - id: prettier-package-xml @@ -65,22 +66,6 @@ repos: - id: black args: [--line-length=100] - - repo: https://github.com/PyCQA/flake8 - rev: 4.0.1 - hooks: - - id: flake8 - additional_dependencies: - [ - flake8-blind-except, - flake8-builtins, - flake8-class-newline, - flake8-comprehensions, - flake8-deprecated, - flake8-docstrings, - flake8-import-order, - flake8-quotes, - ] - - repo: https://github.com/pre-commit/mirrors-clang-format rev: v13.0.1 hooks: diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index fa447301d36c3..7387ef6bb1e9f 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -17,8 +17,8 @@ tier4_debug_msgs launch_ros - rclpy python3-rtree + rclpy ament_lint_auto autoware_lint_common From 39fe090a80690fe7ba617ef31776af9fa50ee079 Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Wed, 5 Jun 2024 16:28:19 +0900 Subject: [PATCH 25/26] fix(obstacle_avoidance_planner): add empty check (#1285) * fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../obstacle_avoidance_planner/src/utils.cpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..57eef51c89549 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -343,26 +343,29 @@ std::vector interpolate2DTraj const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); // spline interpolation - const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; + std::vector interpolated_points; + try { + const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); + const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } } - } - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + } catch (const std::invalid_argument & e) { + return std::vector{}; } - return interpolated_points; } From 35e1dde7f656ff070e9f67adf1f30a41de40be14 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 7 Jun 2024 15:23:50 +0900 Subject: [PATCH 26/26] fix(virtual traffic light): suppress lauch (#1290) * suppress launch Signed-off-by: Yuki Takagi * add existence check Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> --- .../virtual_traffic_light/manager.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 7ac6918f13ed2..d5a302c3275c6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include #include #include @@ -81,11 +84,28 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + tier4_autoware_utils::LineString2d ego_path_linestring; + for (const auto & path_point : path.points) { + ego_path_linestring.push_back( + tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); + } + for (const auto & m : getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr())) { + const auto stop_line_opt = m.first->getStopLine(); + if (!stop_line_opt) { + RCLCPP_FATAL( + logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!", + m.first->id()); + continue; + } + // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); - if (!isModuleRegistered(module_id)) { + if ( + !isModuleRegistered(module_id) && + boost::geometry::intersects( + ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_));