Ros2 v0.8.0 velodyne monitor (autowarefoundation#285)

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(-Wno-unused-parameter -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
+)

system/velodyne_monitor/config/velodyne_monitor.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

system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp

@@ -0,0 +1,125 @@
+// 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 <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+// 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);
+
+  /**
+   * @brief timer callback
+   * @param [in] event timing information
+   */
+  void onTimer();
+
+  rclcpp::TimerBase::SharedPtr timer_;                     //!< @brief timer
+  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_

system/velodyne_monitor/launch/velodyne_monitor.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="config_file" default="$(find-pkg-share velodyne_monitor)/config/velodyne_monitor.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 config_file)" />
+    <param name="ip_address" value="$(var ip_address)" />
+  </node>
+</launch>

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 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>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>