Skip to content

Commit

Permalink
Merge commit 'eb599aec607084113cf0994888031ebe6065468a' into 258-impr…
Browse files Browse the repository at this point in the history 
…ove-occlusion-spot-detection-area-polygon
  • Loading branch information
@taikitanaka3
taikitanaka3 committed Feb 11, 2022
2 parents e7e09cd + eb599ae commit 689b6f4
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 25 deletions.
22 changes: 13 additions & 9 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1682,22 +1682,26 @@ 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);
while (lanelet_at_right) {
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());
}
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion system/system_monitor/config/hdd_monitor.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
16 changes: 12 additions & 4 deletions system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {}
};
Expand Down Expand Up @@ -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
Expand Down
56 changes: 45 additions & 11 deletions system/system_monitor/src/hdd_monitor/hdd_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,24 +156,24 @@ 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));
error_str = "hdd_reader error";
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<float>(itrh->second.temp_);
float temp = static_cast<float>(hdd_itr->second.temp_);

level = DiagStatus::OK;
if (temp >= itr->second.temp_error_) {
Expand All @@ -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);
Expand Down Expand Up @@ -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<std::string>(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<float>(prefix + ".temp_warn");
param.temp_error_ = declare_parameter<float>(prefix + ".temp_error");
param.free_warn_ = declare_parameter<int>(prefix + ".free_warn");
param.free_error_ = declare_parameter<int>(prefix + ".free_error");
const auto name = declare_parameter<std::string>(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_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(HDDMonitor)

0 comments on commit 689b6f4

Please sign in to comment.