diff --git a/system/velodyne_monitor/CMakeLists.txt b/system/velodyne_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..a8a037e83e044 --- /dev/null +++ b/system/velodyne_monitor/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(velodyne_monitor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Target executable +ament_auto_add_executable(velodyne_monitor + src/velodyne_monitor_node.cpp + src/velodyne_monitor.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(velodyne_monitor cpprest crypto fmt) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/velodyne_monitor/Readme.md b/system/velodyne_monitor/Readme.md new file mode 100644 index 0000000000000..020095d6a5280 --- /dev/null +++ b/system/velodyne_monitor/Readme.md @@ -0,0 +1,63 @@ +# velodyne_monitor + +## Purpose + +This node monitors the status of Velodyne LiDARs. +The result of the status is published as diagnostics. + +## Inner-workings / Algorithms + +The status of Velodyne LiDAR can be retrieved from `http://[ip_address]/cgi/{info, settings, status, diag}.json`. + +The types of abnormal status and corresponding diagnostics status are following. + +| Abnormal status | Diagnostic status | +| --------------------------------------------------- | ----------------- | +| No abnormality | OK | +| Top board temperature is too cold | ERROR | +| Top board temperature is cold | WARN | +| Top board temperature is too hot | ERROR | +| Top board temperature is hot | WARN | +| Bottom board temperature is too cold | ERROR | +| Bottom board temperature is cold | WARN | +| Bottom board temperature is too hot | ERROR | +| Bottom board temperature is hot | WARN | +| Rpm(Rotations per minute) of the motor is too low | ERROR | +| Rpm(Rotations per minute) of the motor is low | WARN | +| Connection error (cannot get Velodyne LiDAR status) | ERROR | + +## Inputs / Outputs + +### Input + +None + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +### Node Parameters + +| Name | Type | Default Value | Description | +| --------- | ------ | ------------- | --------------------------------------------------------- | +| `timeout` | double | 0.5 | Timeout for HTTP request to get Velodyne LiDAR status [s] | + +### Core Parameters + +| Name | Type | Default Value | Description | +| ----------------- | ------ | --------------- | ------------------------------------------------------------------------------------------------------------------------- | +| `ip_address` | string | "192.168.1.201" | IP address of target Velodyne LiDAR | +| `temp_cold_warn` | double | -5.0 | If the temperature of Velodyne LiDAR is lower than this value, the diagnostics status becomes WARN [°C] | +| `temp_cold_error` | double | -10.0 | If the temperature of Velodyne LiDAR is lower than this value, the diagnostics status becomes ERROR [°C] | +| `temp_hot_warn` | double | 75.0 | If the temperature of Velodyne LiDAR is higher than this value, the diagnostics status becomes WARN [°C] | +| `temp_hot_error` | double | 80.0 | If the temperature of Velodyne LiDAR is higher than this value, the diagnostics status becomes ERROR [°C] | +| `rpm_ratio_warn` | double | 0.80 | If the rpm rate of the motor (= current rpm / default rpm) is lower than this value, the diagnostics status becomes WARN | +| `rpm_ratio_error` | double | 0.70 | If the rpm rate of the motor (= current rpm / default rpm) is lower than this value, the diagnostics status becomes ERROR | + +## Assumptions / Known limits + +TBD. diff --git a/system/velodyne_monitor/config/velodyne_monitor.param.yaml b/system/velodyne_monitor/config/velodyne_monitor.param.yaml new file mode 100644 index 0000000000000..ce513578893a5 --- /dev/null +++ b/system/velodyne_monitor/config/velodyne_monitor.param.yaml @@ -0,0 +1,10 @@ +--- +/**: + ros__parameters: + timeout: 5.0 + temp_cold_warn: -5.0 + temp_cold_error: -10.0 + temp_hot_warn: 75.0 + temp_hot_error: 80.0 + rpm_ratio_warn: 0.80 + rpm_ratio_error: 0.70 diff --git a/system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp b/system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp new file mode 100644 index 0000000000000..99f63217f5f3e --- /dev/null +++ b/system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp @@ -0,0 +1,118 @@ +// Copyright 2020 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 VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ +#define VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ + +/** + * @file velodyne_monitor.hpp + * @brief Velodyne monitor class + */ + +#include +#include + +#include +#include +#include +#include + +// Include after diagnostic_updater because it causes errors +#include + +namespace http = web::http; +namespace client = web::http::client; +namespace json = web::json; + +class VelodyneMonitor : public rclcpp::Node +{ +public: + /** + * @brief constructor + */ + VelodyneMonitor(); + +protected: + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + + /** + * @brief obtain JSON-formatted diagnostic status and check connection + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkConnection( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check the temperature of the top and bottom board + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkTemperature( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check the motor rpm + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkMotorRpm( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief send an HTTP-GET request + * @param [in] path_query string containing the path, query + * @param [out] res an HTTP response + * @param [out] err_msg diagnostic message passed directly to diagnostic publish calls + * @return true on success, false on error + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + bool requestGET(const std::string & path_query, http::http_response & res, std::string & err_msg); + + /** + * @brief convert raw diagnostic data to usable temperature value + * @param [in] raw raw diagnostic data + * @return usable temperature value + */ + float convertTemperature(int raw); + + diagnostic_updater::Updater updater_; //!< @brief updater class which advertises to /diagnostics + std::unique_ptr client_; //!< @brief HTTP client class + json::value info_json_; //!< @brief values of info.json + json::value diag_json_; //!< @brief values of diag.json + json::value status_json_; //!< @brief values of status.json + json::value settings_json_; //!< @brief values of settings.json + bool diag_json_received_; //!< @brief flag of diag.json received + + std::string ip_address_; //!< @brief Network IP address of sensor + double timeout_; //!< @brief timeout parameter + float temp_cold_warn_; //!< @brief the cold temperature threshold to generate a warning + float temp_cold_error_; //!< @brief the cold temperature threshold to generate an error + float temp_hot_warn_; //!< @brief the hot temperature threshold to generate a warning + float temp_hot_error_; //!< @brief the hot temperature threshold to generate an error + float rpm_ratio_warn_; //!< @brief the rpm threshold(%) to generate a warning + float rpm_ratio_error_; //!< @brief the rpm threshold(%) to generate an error + + /**const ros::TimerEvent & event + * @brief RPM status messages + */ + const std::map rpm_dict_ = { + {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "RPM low"}, {DiagStatus::ERROR, "RPM too low"}}; +}; + +#endif // VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ diff --git a/system/velodyne_monitor/launch/velodyne_monitor.launch.xml b/system/velodyne_monitor/launch/velodyne_monitor.launch.xml new file mode 100644 index 0000000000000..ecb623604db4c --- /dev/null +++ b/system/velodyne_monitor/launch/velodyne_monitor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/velodyne_monitor/package.xml b/system/velodyne_monitor/package.xml new file mode 100644 index 0000000000000..3cc591a0c8122 --- /dev/null +++ b/system/velodyne_monitor/package.xml @@ -0,0 +1,26 @@ + + + velodyne_monitor + 0.1.0 + The velodyne_monitor package + Fumihito Ito + + Apache License 2.0 + + ament_cmake_auto + + crypto++ + diagnostic_msgs + diagnostic_updater + fmt + libcpprest-dev + rclcpp + std_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/velodyne_monitor/src/velodyne_monitor.cpp b/system/velodyne_monitor/src/velodyne_monitor.cpp new file mode 100644 index 0000000000000..e8576a3a63066 --- /dev/null +++ b/system/velodyne_monitor/src/velodyne_monitor.cpp @@ -0,0 +1,231 @@ +// Copyright 2020 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. + +/** + * @file velodyne_monitor.cpp + * @brief Velodyne monitor class + */ + +#include "velodyne_monitor/velodyne_monitor.hpp" + +#include + +#include +#include +#include +#include +#include + +#define FMT_HEADER_ONLY +#include + +VelodyneMonitor::VelodyneMonitor() : Node("velodyne_monitor"), updater_(this) +{ + timeout_ = declare_parameter("timeout", 0.5); + ip_address_ = declare_parameter("ip_address", "192.168.1.201"); + temp_cold_warn_ = declare_parameter("temp_cold_warn", -5.0); + temp_cold_error_ = declare_parameter("temp_cold_error", -10.0); + temp_hot_warn_ = declare_parameter("temp_hot_warn", 75.0); + temp_hot_error_ = declare_parameter("temp_hot_error", 80.0); + rpm_ratio_warn_ = declare_parameter("rpm_ratio_warn", 0.80); + rpm_ratio_error_ = declare_parameter("rpm_ratio_error", 0.70); + + updater_.add("velodyne_connection", this, &VelodyneMonitor::checkConnection); + updater_.add("velodyne_temperature", this, &VelodyneMonitor::checkTemperature); + updater_.add("velodyne_rpm", this, &VelodyneMonitor::checkMotorRpm); + + auto config = client::http_client_config(); + int timeout = timeout_ * 1000; + config.set_timeout(std::chrono::milliseconds(timeout)); + + // Creates a new http_client connected to specified uri + client_.reset(new client::http_client("http://" + ip_address_, config)); + + updater_.setHardwareID("velodyne"); +} + +void VelodyneMonitor::checkConnection(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + http::http_response res; + std::string err_msg = ""; + + // Sends an HTTP-GET request + if (!requestGET("/cgi/info.json", res, err_msg)) { + stat.summary(DiagStatus::ERROR, err_msg); + return; + } + + // Extracts the body of the request message into a json value + try { + info_json_ = res.extract_json().get(); + } catch (const web::http::http_exception & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + stat.summary(DiagStatus::ERROR, ex.what()); + return; + } + + updater_.setHardwareIDf( + "%s: %s", info_json_["model"].as_string().c_str(), info_json_["serial"].as_string().c_str()); + + stat.summary(DiagStatus::OK, "OK"); +} + +void VelodyneMonitor::checkTemperature(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int level_top = DiagStatus::OK; + int level_bot = DiagStatus::OK; + std::vector msg; + std::string err_msg = ""; + + http::http_response res; + diag_json_received_ = false; + + // Sends an HTTP-GET request + if (!requestGET("/cgi/diag.json", res, err_msg)) { + stat.summary(DiagStatus::ERROR, err_msg); + return; + } + + diag_json_received_ = true; + + // Extracts the body of the request message into a json value + try { + diag_json_ = res.extract_json().get(); + } catch (const web::http::http_exception & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + stat.summary(DiagStatus::ERROR, ex.what()); + return; + } + + const float top_temp = + convertTemperature(diag_json_["volt_temp"]["top"]["lm20_temp"].as_integer()); + const float bot_temp = + convertTemperature(diag_json_["volt_temp"]["bot"]["lm20_temp"].as_integer()); + + // Check top board temperature + if (top_temp < temp_cold_error_) { + level_top = DiagStatus::ERROR; + msg.emplace_back("Top board temperature too cold"); + } else if (top_temp < temp_cold_warn_) { + level_top = DiagStatus::WARN; + msg.emplace_back("Top board temperature cold"); + } else if (top_temp > temp_hot_error_) { + level_top = DiagStatus::ERROR; + msg.emplace_back("Top board temperature too hot"); + } else if (top_temp > temp_hot_warn_) { + level_top = DiagStatus::WARN; + msg.emplace_back("Top board temperature hot"); + } + + // Check bottom board temperature + if (bot_temp < temp_cold_error_) { + level_bot = DiagStatus::ERROR; + msg.emplace_back("Bottom board temperature too cold"); + } else if (bot_temp < temp_cold_warn_) { + level_bot = DiagStatus::WARN; + msg.emplace_back("Bottom board temperature cold"); + } else if (bot_temp > temp_hot_error_) { + level_bot = DiagStatus::ERROR; + msg.emplace_back("Bottom board temperature too hot"); + } else if (bot_temp > temp_hot_warn_) { + level_bot = DiagStatus::WARN; + msg.emplace_back("Bottom board temperature hot"); + } + + stat.addf("Top board", "%.3lf DegC", top_temp); + stat.addf("Bottom board", "%.3lf DegC", bot_temp); + + if (msg.empty()) { + msg.emplace_back("OK"); + } + + stat.summary(std::max(level_top, level_bot), boost::algorithm::join(msg, ", ")); +} + +void VelodyneMonitor::checkMotorRpm(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int level = DiagStatus::OK; + http::http_response res; + std::string err_msg = ""; + + // Sends an HTTP-GET request + if (!requestGET("/cgi/settings.json", res, err_msg)) { + stat.summary(DiagStatus::ERROR, err_msg); + return; + } + + // Extracts the body of the request message into a json value + try { + settings_json_ = res.extract_json().get(); + } catch (const web::http::http_exception & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + stat.summary(DiagStatus::ERROR, ex.what()); + return; + } + + // Sends an HTTP-GET request + if (!requestGET("/cgi/status.json", res, err_msg)) { + stat.summary(DiagStatus::ERROR, err_msg); + return; + } + + // Extracts the body of the request message into a json value + try { + status_json_ = res.extract_json().get(); + } catch (const web::http::http_exception & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + stat.summary(DiagStatus::ERROR, ex.what()); + return; + } + + const double setting = settings_json_["rpm"].as_double(); + const double rpm = status_json_["motor"]["rpm"].as_double(); + const double ratio = rpm / setting; + + if (ratio < rpm_ratio_error_) { + level = DiagStatus::ERROR; + } else if (ratio < rpm_ratio_warn_) { + level = DiagStatus::WARN; + } + + stat.addf("Ratio", "%.2lf %%", ratio * 1e2); + stat.addf("Setting", "%.0f", setting); + stat.addf("Current", "%.0f", rpm); + + stat.summary(level, rpm_dict_.at(level)); +} + +bool VelodyneMonitor::requestGET( + const std::string & path_query, http::http_response & res, std::string & err_msg) +{ + // Asynchronously sends an HTTP request + try { + res = client_->request(http::methods::GET, path_query).get(); + } catch (const std::exception & e) { + err_msg = e.what(); + return false; + } + + if (res.status_code() != web::http::status_codes::OK) { + err_msg = fmt::format("{}: {}", res.status_code(), res.reason_phrase().c_str()); + return false; + } + + return true; +} + +float VelodyneMonitor::convertTemperature(int raw) +{ + return std::sqrt(2.1962e6 + (1.8639 - static_cast(raw) * 5.0 / 4096) / 3.88e-6) - 1481.96; +} diff --git a/system/velodyne_monitor/src/velodyne_monitor_node.cpp b/system/velodyne_monitor/src/velodyne_monitor_node.cpp new file mode 100644 index 0000000000000..235940ad33160 --- /dev/null +++ b/system/velodyne_monitor/src/velodyne_monitor_node.cpp @@ -0,0 +1,34 @@ +// Copyright 2020 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. + +/** + * @file velodyne_monitor_node.cpp + * @brief Velodyne monitor node class + */ + +#include "velodyne_monitor/velodyne_monitor.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}