From 12ef731c20400cd2ceecb9258b72c65b94e8dbaa Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Wed, 9 Feb 2022 22:36:03 +0900 Subject: [PATCH 1/2] feat(system_monitor): handle parameter as mount point (#259) Signed-off-by: ito-san --- .../config/hdd_monitor.param.yaml | 2 +- .../hdd_monitor/hdd_monitor.hpp | 16 ++++-- .../src/hdd_monitor/hdd_monitor.cpp | 56 +++++++++++++++---- 3 files changed, 58 insertions(+), 16 deletions(-) diff --git a/system/system_monitor/config/hdd_monitor.param.yaml b/system/system_monitor/config/hdd_monitor.param.yaml index 32d3a425b1898..817ff68814a56 100644 --- a/system/system_monitor/config/hdd_monitor.param.yaml +++ b/system/system_monitor/config/hdd_monitor.param.yaml @@ -4,7 +4,7 @@ num_disks: 1 disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} disk0: - name: /dev/sda3 + name: / temp_warn: 55.0 temp_error: 70.0 free_warn: 5120 # MB(8hour) diff --git a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp index 0dcee7de54aad..204c52fc23705 100644 --- a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp +++ b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp @@ -32,10 +32,11 @@ */ struct HDDParam { - float temp_warn_; //!< @brief HDD temperature(DegC) to generate warning - float temp_error_; //!< @brief HDD temperature(DegC) to generate error - int free_warn_; //!< @brief HDD free space(MB) to generate warning - int free_error_; //!< @brief HDD free space(MB) to generate error + std::string device_; //!< @brief device + float temp_warn_; //!< @brief HDD temperature(DegC) to generate warning + float temp_error_; //!< @brief HDD temperature(DegC) to generate error + int free_warn_; //!< @brief HDD free space(MB) to generate warning + int free_error_; //!< @brief HDD free space(MB) to generate error HDDParam() : temp_warn_(55.0), temp_error_(70.0), free_warn_(5120), free_error_(100) {} }; @@ -86,6 +87,13 @@ class HDDMonitor : public rclcpp::Node */ void getHDDParams(); + /** + * @brief get device name from mount point + * @param [in] mount_point mount point + * @return device name + */ + std::string getDeviceFromMountPoint(const std::string & mount_point); + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 20be9d3010d1d..b863a19547b39 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -156,8 +156,8 @@ void HDDMonitor::checkTemp(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++index) { // Retrieve HDD information - auto itrh = list.find(itr->first); - if (itrh == list.end()) { + auto hdd_itr = list.find(itr->second.device_); + if (hdd_itr == list.end()) { stat.add(fmt::format("HDD {}: status", index), "hdd_reader error"); stat.add(fmt::format("HDD {}: name", index), itr->first.c_str()); stat.add(fmt::format("HDD {}: hdd_reader", index), strerror(ENOENT)); @@ -165,15 +165,15 @@ void HDDMonitor::checkTemp(diagnostic_updater::DiagnosticStatusWrapper & stat) continue; } - if (itrh->second.error_code_ != 0) { + if (hdd_itr->second.error_code_ != 0) { stat.add(fmt::format("HDD {}: status", index), "hdd_reader error"); stat.add(fmt::format("HDD {}: name", index), itr->first.c_str()); - stat.add(fmt::format("HDD {}: hdd_reader", index), strerror(itrh->second.error_code_)); + stat.add(fmt::format("HDD {}: hdd_reader", index), strerror(hdd_itr->second.error_code_)); error_str = "hdd_reader error"; continue; } - float temp = static_cast(itrh->second.temp_); + float temp = static_cast(hdd_itr->second.temp_); level = DiagStatus::OK; if (temp >= itr->second.temp_error_) { @@ -183,9 +183,9 @@ void HDDMonitor::checkTemp(diagnostic_updater::DiagnosticStatusWrapper & stat) } stat.add(fmt::format("HDD {}: status", index), temp_dict_.at(level)); - stat.add(fmt::format("HDD {}: name", index), itr->first.c_str()); - stat.add(fmt::format("HDD {}: model", index), itrh->second.model_.c_str()); - stat.add(fmt::format("HDD {}: serial", index), itrh->second.serial_.c_str()); + stat.add(fmt::format("HDD {}: name", index), itr->second.device_.c_str()); + stat.add(fmt::format("HDD {}: model", index), hdd_itr->second.model_.c_str()); + stat.add(fmt::format("HDD {}: serial", index), hdd_itr->second.serial_.c_str()); stat.addf(fmt::format("HDD {}: temperature", index), "%.1f DegC", temp); whole_level = std::max(whole_level, level); @@ -301,18 +301,52 @@ void HDDMonitor::getHDDParams() const auto num_disks = this->declare_parameter("num_disks", 0); for (auto i = 0; i < num_disks; ++i) { const auto prefix = "disks.disk" + std::to_string(i); + const auto name = declare_parameter(prefix + ".name"); + + // Get device name from mount point + const auto device_name = getDeviceFromMountPoint(name); + if (device_name.empty()) { + continue; + } + HDDParam param; param.temp_warn_ = declare_parameter(prefix + ".temp_warn"); param.temp_error_ = declare_parameter(prefix + ".temp_error"); param.free_warn_ = declare_parameter(prefix + ".free_warn"); param.free_error_ = declare_parameter(prefix + ".free_error"); - const auto name = declare_parameter(prefix + ".name"); - hdd_params_[name] = param; + // Remove index number for passing device name to hdd-reader + const std::regex pattern("\\d+$"); + param.device_ = std::regex_replace(device_name, pattern, ""); + hdd_params_[device_name] = param; - hdd_devices_.push_back(name); + hdd_devices_.push_back(param.device_); } } +std::string HDDMonitor::getDeviceFromMountPoint(const std::string & mount_point) +{ + std::string ret; + bp::ipstream is_out; + bp::ipstream is_err; + + bp::child c( + "/bin/sh", "-c", fmt::format("findmnt -n -o SOURCE {}", mount_point.c_str()), + bp::std_out > is_out, bp::std_err > is_err); + c.wait(); + + if (c.exit_code() != 0) { + RCLCPP_ERROR(get_logger(), "Failed to execute findmnt. %s", mount_point.c_str()); + return ""; + } + + if (!std::getline(is_out, ret)) { + RCLCPP_ERROR(get_logger(), "Failed to find device name. %s", mount_point.c_str()); + return ""; + } + + return ret; +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(HDDMonitor) From eb599aec607084113cf0994888031ebe6065468a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 10 Feb 2022 10:37:27 +0900 Subject: [PATCH 2/2] fix(behavior_path_planner): crash when drivable area expanding (#365) This is to fix the behavior path planner crash when the drivable area is expanding towards opposite direction lane Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 5649c7e2a4025..1922cb97b1286 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1682,11 +1682,13 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c extended_lanelets.push_back(lanelet_at_left.get()); lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } - auto lanelet_at_right = - planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); - while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + if (lanelet_at_left) { + auto lanelet_at_right = + planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); + while (lanelet_at_right) { + extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + } } } else { auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet); @@ -1694,10 +1696,12 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c extended_lanelets.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } - auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); - while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + if (lanelet_at_right) { + auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); + while (lanelet_at_left) { + extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + } } } }