feat: add velodyne_monitor package (autowarefoundation#17)

* Ros2 v0.8.0 velodyne monitor (autowarefoundation#285) * Rename ROS-related .yaml to .param.yaml (autowarefoundation#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix overlooked parameters Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add use_sim-time option (autowarefoundation#454) * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add exception handling for extract_json() (autowarefoundation#1779) * Add exception handling for extract_json() Signed-off-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp> * Add diagnostics error when catching exception Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Fix compiler warnings (autowarefoundation#1837) * Fix -Wunused-private-field Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-variable Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wformat-security Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Winvalid-constexpr Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wdelete-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Winconsistent-missing-override Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix "invalid application of 'sizeof' to an incomplete type" Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore -Wgnu-anonymous-struct and -Wnested-anon-types Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix lint Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore -Wno-deprecated-declarations in CUDA-related packages Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove duplicated update (autowarefoundation#2072) (autowarefoundation#2084) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Fix velodyne monitor config file variable name (autowarefoundation#2090) (autowarefoundation#2092) Signed-off-by: j4tfwm6z <proj-jpntaxi@tier4.jp> Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * remove COLCON_IGNORE in system_packages and map_tf_generator (autowarefoundation#532) * [Velodyne monitor]add readme (autowarefoundation#570) * add readme * change the description * Update system/velodyne_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/velodyne_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * Update system/velodyne_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * velodyne -> velodyne lidar * Update system/velodyne_monitor/Readme.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update system/velodyne_monitor/Readme.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Takayuki AKAMINE <takayuki.akamine@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
badai-nguyen · Dec 3, 2021 · 4dcfe68 · 4dcfe68
1 parent 05b2bba
commit 4dcfe68
Show file tree

Hide file tree

Showing 8 changed files with 526 additions and 0 deletions.
diff --git a/system/velodyne_monitor/CMakeLists.txt 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
@@ -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
@@ -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
@@ -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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+// Include after diagnostic_updater because it causes errors
+#include <cpprest/http_client.h>
+
+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::http_client> 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<int, const char *> 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
@@ -0,0 +1,9 @@
+<launch>
+  <arg name="velodyne_monitor_param_file" default="$(find-pkg-share velodyne_monitor)/config/velodyne_monitor.param.yaml"/>
+  <arg name="ip_address" default="192.168.1.201" />
+
+  <node pkg="velodyne_monitor" exec="velodyne_monitor" name="velodyne_monitor" output="log" respawn="true">
+    <param from="$(var velodyne_monitor_param_file)" />
+    <param name="ip_address" value="$(var ip_address)" />
+  </node>
+</launch>
diff --git a/system/velodyne_monitor/package.xml b/system/velodyne_monitor/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>velodyne_monitor</name>
+  <version>0.1.0</version>
+  <description>The velodyne_monitor package</description>
+  <maintainer email="fumihito.ito@tier4.jp">Fumihito Ito</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>crypto++</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>fmt</depend>
+  <depend>libcpprest-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>