Skip to content

Commit

Permalink
feat: add velodyne_monitor package (#17)
Browse files Browse the repository at this point in the history 
* Ros2 v0.8.0 velodyne monitor (#285)

* Rename ROS-related .yaml to .param.yaml (#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 (#454)

* Unify Apache-2.0 license name (#1242)

* Remove use_sim_time for set_parameter (#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add exception handling for extract_json() (#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 (#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 (#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 (#2072) (#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 (#2090) (#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 (#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 (#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* remove COLCON_IGNORE in system_packages and map_tf_generator (#532)

* [Velodyne monitor]add readme (#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>
  • Loading branch information
@tkimura4 @wep21
@kenji-miyake @kmiya @takam5f2 @h-ohta @tier4-autoware-private-bot @j4tfwm6z @1222-takeshi @taikitanaka3
10 people authored Dec 3, 2021
1 parent f8e1dcd commit d3fa72a
Show file tree
Hide file tree
Showing 8 changed files with 526 additions and 0 deletions.
35 changes: 35 additions & 0 deletions system/velodyne_monitor/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
)
63 changes: 63 additions & 0 deletions system/velodyne_monitor/Readme.md
View file
Original file line number Diff line number Diff line change
@@ -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.
10 changes: 10 additions & 0 deletions system/velodyne_monitor/config/velodyne_monitor.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
118 changes: 118 additions & 0 deletions system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp
View file
Original file line number Diff line number Diff line change
@@ -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_
9 changes: 9 additions & 0 deletions system/velodyne_monitor/launch/velodyne_monitor.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
26 changes: 26 additions & 0 deletions system/velodyne_monitor/package.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
Loading

0 comments on commit d3fa72a

Please sign in to comment.