diff --git a/ros/src/common/libs/diagnostics_lib/README.md b/ros/src/common/libs/diagnostics_lib/README.md deleted file mode 100644 index e155dc0a384..00000000000 --- a/ros/src/common/libs/diagnostics_lib/README.md +++ /dev/null @@ -1,146 +0,0 @@ -# diagnostic_lib - -## diagnostic packages for Autoware. - -### packages -1. diag_lib -packages contain diag_manager. - -1. diag_msgs -original messages for diagnostic - -1. fake_autoware_nodes -test fake nodes and samples - -### How to test -``` -roslaunch fake_autoware_nodes diag_sample.launch -``` - -### How to use -1. add diag_lib,diag_msgs to your package dependency. - -1. #include to your source code. - -1. create diag_manager class instance in your source code. - -1. define error code to yaml file (sample: /config/error_code_sample_config.yaml) - -### Compile options -1. -DSTRICT_ERROR_CODE_CHECK=(ON or OFF) -if you set this option ON, check error code infomation and when it was wrong, error message was shown and the wrong node was killed. - -### How it works -diag_manager instance which attatched to your C++ ROS node publish /diag topic (type:diag_msgs/diag_error). -You can define errors in .yaml setting file. -Each diag_manager and watchdog node read .yaml setting file and load error codes. -The watchdog node check allive or dead all the nodes in the .yaml setting file and aggregate /diag topic which comes from diag_managers. - -![rosgraph](rosgraph.png "rosgraph") - -### How to attach diag_manager class -Please read source codes in fake_autoware_nodes - -### required parameters(rosparam) -error_code_config_path (type:String) : full path of error code config file path. - -#### public member functions -#### constructer and destructer - -1. constructer -``` -diag_manager::diag_manager(); -``` - -2. destructor -``` -diag_manager::diag_manager(); -``` - -#### diag functions -Note : argument num is the error number you defined in the error code setting yaml file. -1. DIAG_ASSERT_VALUE_RANGE -``` -template -void DIAG_ASSERT_VALUE_RANGE(T min, T max, T value, int num) -``` -If value is not in the range, send diag message. -When you use this function , you expected the value satisfy the conditional expression "min < value < max". -num is the error number you defined in the error code setting yaml file. - -2. DIAG_ASSERT_VALUE_MIN -``` -template -void DIAG_ASSERT_VALUE_MIN(T min, T value, int num) -``` -If value is not in the range, send diag message. -When you use this function , you expected the value satisfy the conditional expression "min < value". -num is the error number you defined in the error code setting yaml file. - -3. DIAG_ASSERT_VALUE_MAX -``` -template -void DIAG_ASSERT_VALUE_MIN(T max, T value, int num) -``` -If value is not in the range, send diag message. -When you use this function , you expected the value satisfy the conditional expression "max < value". -num is the error number you defined in the error code setting yaml file. - -4. DIAG_ASSERT_EXCEPTION -``` -template -void DIAG_ASSERT_EXCEPTION(T exception,int num) -``` -You shold call this function in catch block in your source code. -When this function was called, diag_manager send a EXCEPTION diag message. - -5. DIAG_RESOURCE -``` -void DIAG_RESOURCE(std::string target_resource_path, int num); -``` -When the function was called, diag_manager check the target resource file exist. -If diag_manager failed to find the file, diag_manager kill the target ROS node. -I strongly recommend that you should call this function in the initialize function. -num is the error number you defined in the error code setting yaml file. - -6. DIAG_RATE_CHECK -``` -void DIAG_RATE_CHECK(int num); -``` -Put this function where you want to know operation cycle. -num is the error number you defined in the error code setting yaml file. - -7. DIAG_LOW_RELIABILITY -``` -void DIAG_LOW_RELIABILITY(int num); -``` -You call this function when your node output is low reliability. -num is the error number you defined in the error code setting yaml file. - -#### logging function -1. WRITE_LOG -Write log file to /tmp/Autoware/Diag/Log//log.txt -The log remains in such format. -``` -[2018-09-13T03:25:25.340543] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:25.341312] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.441295] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.541326] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.641427] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.741318] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.841311] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:25.941436] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:26.041322] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:26.141353] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:26.340464] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:27.340491] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:28.241331] : in /ndt_matching: exception was catched -[2018-09-13T03:25:28.241375] : in /ndt_matching: Divided by zero. -[2018-09-13T03:25:28.340549] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:29.340556] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:30.340551] : in /ndt_matching: topic /nft_matching/data subscribe rate was low (Warn) -[2018-09-13T03:25:30.341377] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:30.441393] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:30.541348] : in /ndt_matching: The input value hogehoge is out of range. -[2018-09-13T03:25:30.641382] : in /ndt_matching: The input value hogehoge is out of range. -``` \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/CHANGELOG.rst b/ros/src/common/libs/diagnostics_lib/diag_lib/CHANGELOG.rst deleted file mode 100644 index 122fa7d6e89..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/CHANGELOG.rst +++ /dev/null @@ -1,28 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package diag_lib -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.10.0 (2019-01-17) -------------------- - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- -* add filter to the fake_autoware_node -* add filter function by using node number -* dd diag_filter to the fake_subscriber -* add disable mode for diag_manager -* add diag_lib - add csv writer - remove unused comment - fix maintainer names - fix email address - fix dependency - fix buildtool_depend, remove unused lines from find_package -* Contributors: Masaya Kataoka - -* add csv writer -* add diag_lib -* Contributors: Masaya Kataoka diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/CMakeLists.txt b/ros/src/common/libs/diagnostics_lib/diag_lib/CMakeLists.txt deleted file mode 100644 index dbffd972061..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(diag_lib) - -add_compile_options(-std=c++11) - -find_package(catkin REQUIRED COMPONENTS - diag_msgs - roscpp -) - -find_package(Boost REQUIRED COMPONENTS filesystem) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) - -option(STRICT_ERROR_CODE_CHECK "enables strict error code check" OFF) -if(STRICT_ERROR_CODE_CHECK) - add_definitions(-DSTRICT_ERROR_CODE_CHECK) -endif() - -catkin_package( - INCLUDE_DIRS include - LIBRARIES diag_manager diag_filter - CATKIN_DEPENDS diag_msgs roscpp -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -set(watchdog_node_src src/watchdog_node.cpp src/watchdog.cpp src/diag_manager.cpp src/rate_checker.cpp src/diag_subscriber.cpp) -add_executable(watchdog_node ${watchdog_node_src}) -target_link_libraries(watchdog_node ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) -add_dependencies(watchdog_node ${catkin_EXPORTED_TARGETS}) - -set(diag_manager_src src/diag_manager.cpp src/rate_checker.cpp src/diag_subscriber.cpp) -add_library(diag_manager SHARED ${diag_manager_src}) -target_link_libraries(diag_manager ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) -add_dependencies(diag_manager ${catkin_EXPORTED_TARGETS}) - -set(diag_filter_src src/diag_filter.cpp) -add_library(diag_filter SHARED ${diag_filter_src}) -target_link_libraries(diag_filter ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) -add_dependencies(diag_filter ${catkin_EXPORTED_TARGETS}) - -# CPP Execution programs -set(CPP_EXEC_NAMES watchdog_node) -foreach(cpp_exec_names ${CPP_EXEC_NAMES}) - install(TARGETS ${cpp_exec_names} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -endforeach(cpp_exec_names) - -# include header files -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -# Install all files -foreach(dir config launch) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) - -# Install library -install(TARGETS diag_manager diag_filter - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/config/error_code_sample_config.yaml b/ros/src/common/libs/diagnostics_lib/diag_lib/config/error_code_sample_config.yaml deleted file mode 100644 index 7a7989b3cf3..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/config/error_code_sample_config.yaml +++ /dev/null @@ -1,49 +0,0 @@ -/watchdog_node: - node_number: 0 - errors: - - num: 0 - name: required file does not exist - category: 7 - description: required file does not exist -/pure_pursuit: - node_number: 0 - errors: - - num: 0 - name: out of range - category: 8 - description: The input value hogehoge is out of range. - - num: 1 - name: node is ded - category: 3 - description: /pure_pursuit node is dead -/ndt_matching: - node_number: 1 - errors: - - num: 0 - name: out of range - category: 8 - description: The input value hogehoge is out of range. - - num: 1 - name: exception - category: 2 - description: exception was catched - - num: 2 - name: low subscribe rate - category: 4 - description: topic /nft_matching/data subscribe rate was low (Warn) - threshold: 10.0 - level: warn - - num: 3 - name: low subscribe rate - category: 4 - description: topic /nft_matching/data subscribe rate was low (Error) - threshold: 5.0 - level: error - - num: 4 - name: low reliability - category: 12 - description: low reliability - - num: 5 - name: node is ded - category: 3 - description: /ndt_matching node is dead \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_filter.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_filter.h deleted file mode 100644 index 4312f929d5e..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_filter.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef DIAG_FILTER_H_INCLUDED -#define DIAG_FILTER_H_INCLUDED - -//headers in diag_msgs -#include -#include -#include - -//headers in ROS -#include - -//headers in Boost -#include - -//headers in STL -#include - -class diag_filter -{ -public: - diag_filter(); - ~diag_filter(); - boost::optional filter(diag_msgs::diag diag, std::string target_node); - boost::optional filter(diag_msgs::diag diag, int target_node_number); -private: - std::map node_number_data_; - ros::NodeHandle nh_; - bool check_resource_(std::string target_resource_path); - volatile bool enable_; - std::string error_code_config_path_; -}; -#endif //DIAG_FILTER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_info.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_info.h deleted file mode 100644 index e535ca57995..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_info.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef DIAG_INFO_H_INCLUDED -#define DIAG_INFO_H_INCLUDED - -//headers in diag_lib -#include - -//headers in STL -#include - -//headers in boost -#include - -struct diag_info -{ - const int num; - const std::string name; - const int category; - const std::string description; - const boost::optional threshold; - const boost::optional level; - diag_info(int num_, std::string name_, int category_, std::string description_) - : num(num_), name(name_), category(category_) ,description(description_) , threshold(boost::none), level(boost::none) - {}; - diag_info(int num_, std::string name_, int category_, std::string description_, double threshold_, int level_) - : num(num_), name(name_), category(category_) ,description(description_) , threshold(threshold_), level(level_) - { - - } -}; -#endif //DIAG_INFO_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_macros.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_macros.h deleted file mode 100644 index 8ec29cfc134..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_macros.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef DIAG_MACROS_H_INCLUDED -#define DIAG_MACROS_H_INCLUDED - -#define GET_VARIABLE_NAME(Variable) - -#endif //DIAG_MACROS_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager.h deleted file mode 100644 index c5c099fd19e..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager.h +++ /dev/null @@ -1,137 +0,0 @@ -#ifndef DIAG_MANAGER_H_INCLUDED -#define DIAG_MANAGER_H_INCLUDED - -//headers in diag_lib -#include -#include -#include -#include -#include -#include - -//headers in ROS -#include - -//headers in STL -#include -#include -#include -#include - -//headers in diag_msgs -#include - -//headers in boost -#include -#include -#include -#include -#include -#include - -// headers in YAML-CPP -#include - -class diag_manager -{ -public: - diag_manager(); - ~diag_manager(); - template - void DIAG_ASSERT_VALUE_RANGE(T min, T max, T value, int num) - { - if(enable_diag_ == false) - return; - std::vector required_error_code = {INVALID_INITIAL_VALUE, INVALID_VALUE}; - if(check_error_code(num, required_error_code)) - { - if(value < min) - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - publish_diag_(query_diag_info(num).get()); - return; - } - if(value > max) - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - publish_diag_(query_diag_info(num).get()); - return; - } - } - return; - } - template - void DIAG_ASSERT_VALUE_MIN(T min, T value, int num) - { - if(enable_diag_ == false) - return; - std::vector required_error_code = {INVALID_INITIAL_VALUE, INVALID_VALUE}; - if(check_error_code(num, required_error_code)) - { - if(value < min) - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - publish_diag_(query_diag_info(num).get()); - return; - } - } - return; - } - template - void DIAG_ASSERT_VALUE_MAX(T max, T value, int num) - { - if(enable_diag_ == false) - return; - std::vector required_error_code = {INVALID_INITIAL_VALUE, INVALID_VALUE}; - if(check_error_code(num, required_error_code)) - { - if(value > max) - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - publish_diag_(query_diag_info(num).get()); - return; - } - } - return; - } - template - void DIAG_ASSERT_EXCEPTION(T exception,int num) - { - if(enable_diag_ == false) - return; - std::vector required_error_code = {EXCEPTION}; - if(check_error_code(num, required_error_code)) - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - ADD_DIAG_LOG_WARN(exception.what()); - publish_diag_(query_diag_info(num).get()); - } - return; - } - void DIAG_RESOURCE(std::string target_resource_path, int num); - void DIAG_RATE_CHECK(int num); - void DIAG_LOW_RELIABILITY(int num); - std::vector get_diag_info(){return diag_info_;} - boost::optional query_diag_info(int num); - void WRITE_LOG(); -private: - void ADD_DIAG_LOG_WARN(std::string log_text); - void ADD_DIAG_LOG_ERROR(std::string log_text); - void check_rate_(); - bool check_error_code(int requested_error_code, std::vector right_categories); - void publish_diag_(diag_info info); - // check resource for diag_manager - bool diag_resource(std::string target_resource_path); - volatile bool enable_diag_; - std::vector diag_info_; - std::vector diag_log_; - ros::Publisher diag_pub_; - ros::NodeHandle nh_; - ros::Time timer_; - //config - std::string error_code_config_path_; - YAML::Node error_code_config_; - //rate checker - std::map > checkers_; -}; -#endif //DIAG_MANAGER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager_config.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager_config.h deleted file mode 100644 index 26e3234e491..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_manager_config.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef DIAG_MANGER_CONFIG_H_INCLUDED -#define DIAG_MANGER_CONFIG_H_INCLUDED - -/* -define values for rate checker -*/ -#define RATE_CHECKER_BUFFER_LENGTH 1 -#define RATE_CHECK_FREQUENCY 1.0 - -/* -define for Pub/Sub Operation cycle critical LEVEL -*/ -//#define LEVEL_NORMAL 0 -#define LEVEL_WARN 1 -#define LEVEL_ERROR 2 - -#endif //DIAGMANAGER_CONFIG_H_INCULDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_subscriber.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_subscriber.h deleted file mode 100644 index c138ce8dd39..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/diag_subscriber.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef DIAG_SUBSCRIBER_H_INCLUDED -#define DIAG_SUBSCRIBER_H_INCLUDED - -//headers in STL -#include -#include -#include - -//headers in ROS -#include - -//headers in diag_msgs -#include -#include - -class diag_subscriber -{ - public: - diag_subscriber(std::string target_node,int target_node_number); - ~diag_subscriber(); - diag_msgs::diag_node_errors get_diag_node_errors(); - private: - std::mutex mtx_; - std::vector buffer_; - ros::Subscriber diag_sub_; - ros::NodeHandle nh_; - void callback_(diag_msgs::diag_error msg); - const std::string target_node_; - const int target_node_number_; -}; - -#endif //DIAG_SUBSCRIBER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/error_category.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/error_category.h deleted file mode 100644 index 38f0c5ea891..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/error_category.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef ERROR_CATEGORY_H_INCLUDED -#define ERROR_CATEGORY_H_INCLUDED - -//#define MEMORY_ERROR 0 -//#define SEGMENTATION_FAULT 2 -#define EXCEPTION 2 -#define NODE_IS_DEAD 3 -#define LOW_SUBSCRIBE_RATE 4 -#define LOW_PUBLISH_RATE 5 -#define LOW_OPERATION_CYCLE 6 -#define RESOURCE_NOT_FOUND 7 -#define INVALID_INITIAL_VALUE 8 -#define INVALID_VALUE 9 -#define LOW_RELIABILITY 12 -#define DEVICE_CONNECTION_LOST 13 - -#endif //ERROR_CATEGORY_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/rate_checker.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/rate_checker.h deleted file mode 100644 index fb65459a07d..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/rate_checker.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RATE_CHECKER_H_INCLUDED -#define RATE_CHECKER_H_INCLUDED - -//headers in ROS -#include - -//headers in STL -#include -#include - -//headers in Boost -#include - -class rate_checker -{ - public: - rate_checker(double buffer_length); - ~rate_checker(); - void check(); - boost::optional get_rate(); - private: - ros::Time start_time_; - void update_(); - std::vector data_; - const double buffer_length_; - std::mutex mtx_; -}; -#endif //RATE_CHECKER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/watchdog.h b/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/watchdog.h deleted file mode 100644 index a2f50e9388e..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/include/diag_lib/watchdog.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef WATCHDOG_H_INCLUDED -#define WATCHDOG_H_INCLUDED - -//headers in diag_lib -#include -#include -#include - -//headers in diag_msgs -#include -#include -#include - -//headers in STL -#include -#include - -//headers in YAML-CPP -#include - -//headers in Boost -#include -#include -#include -#include - -class watchdog -{ -public: - watchdog(); - ~watchdog(); - void run(); -private: - ros::NodeHandle nh_; - // parameters - std::string config_filepath_; - double publish_rate_; - // diagnostic manager - diag_manager diag_; - std::map > > watchdog_diag_info_; - std::vector diag_target_nodes_; - std::vector watchdog_target_nodes_; - std::map connection_status_; - std::map > diag_sub_; - ros::Publisher diag_pub_; - void update_connection_status_(); - void publish_diag_(); - void write_error_code_csv_(YAML::Node config); -}; -#endif //WATCHDOG_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/launch/watchdog_sample.launch b/ros/src/common/libs/diagnostics_lib/diag_lib/launch/watchdog_sample.launch deleted file mode 100644 index c0f0d0d941b..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/launch/watchdog_sample.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/package.xml b/ros/src/common/libs/diagnostics_lib/diag_lib/package.xml deleted file mode 100644 index 7f12f177f7a..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - diag_lib - 1.10.0 - The diag_lib package - - Masaya Kataoka - - Apache 2 - - Masaya Kataoka - - catkin - autoware_build_flags - diag_msgs - roscpp - yaml-cpp - diag_msgs - roscpp - diag_msgs - roscpp - - - - diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_filter.cpp b/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_filter.cpp deleted file mode 100644 index e26efc02685..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_filter.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// headers in diag_lib -#include - -// headers in YAML-CPP -#include - -//headers in boost -#include - -diag_filter::diag_filter() -{ - nh_.param("/error_code_config_path", error_code_config_path_, std::string("")); - if(check_resource_(error_code_config_path_)) - { - enable_ = true; - } - else - { - enable_ = false; - return; - } - YAML::Node config = YAML::LoadFile(error_code_config_path_.c_str()); - try - { - for(YAML::const_iterator it=config.begin();it != config.end();++it) - { - YAML::Node single_node_data = config[it->first.as()]; - node_number_data_[it->first.as()] = single_node_data["node_number"].as(); - } - } - catch(...) - { - ROS_ERROR_STREAM("Failed to parse error code yaml file."); - } -} - -diag_filter::~diag_filter() -{ - -} - -bool diag_filter::check_resource_(std::string target_resource_path) -{ - namespace fs = boost::filesystem; - fs::path path(target_resource_path); - boost::system::error_code error; - const bool result = fs::exists(path, error); - if (!result || error) - return false; - return true; -} - -boost::optional diag_filter::filter(diag_msgs::diag diag, std::string target_node) -{ - return filter(diag,node_number_data_[target_node]); -} - -boost::optional diag_filter::filter(diag_msgs::diag diag, int target_node_number) -{ - for(int i=0; i - -diag_manager::diag_manager() -{ - enable_diag_ = false; - nh_.param("/error_code_config_path", error_code_config_path_, std::string("")); - if(!diag_resource(error_code_config_path_)) - { - return; - } - YAML::Node config = YAML::LoadFile(error_code_config_path_.c_str()); - error_code_config_ = config[ros::this_node::getName()]; - try - { - for(const YAML::Node &error : error_code_config_["errors"]) - { - diag_info info(error["num"].as(), error["name"].as(), error["category"].as(), error["description"].as()); - if((info.category == LOW_SUBSCRIBE_RATE) || (info.category == LOW_PUBLISH_RATE) || (info.category == LOW_OPERATION_CYCLE)) - { - if(error["level"].as() == "warn") - { - diag_info detail_info(error["num"].as(), error["name"].as(), error["category"].as(), error["description"].as() ,error["threshold"].as(), LEVEL_WARN); - boost::shared_ptr rate_checker_ptr = boost::make_shared(RATE_CHECKER_BUFFER_LENGTH); - checkers_[info.num] = rate_checker_ptr; - diag_info_.push_back(detail_info); - } - if(error["level"].as() == "error") - { - diag_info detail_info(error["num"].as(), error["name"].as(), error["category"].as(), error["description"].as() ,error["threshold"].as(), LEVEL_ERROR); - boost::shared_ptr rate_checker_ptr = boost::make_shared(RATE_CHECKER_BUFFER_LENGTH); - checkers_[info.num] = rate_checker_ptr; - diag_info_.push_back(detail_info); - } - continue; - } - diag_info_.push_back(info); - } - } - catch(...) - { -#ifdef STRICT_ERROR_CODE_CHECK - ROS_ERROR_STREAM("diag config file format was wrong. kill " + ros::this_node::getName() + ". Please check " << error_code_config_path_); - WRITE_LOG(); - std::exit(-1); -#else - ROS_WARN_STREAM("diag config file format was wrong in " + ros::this_node::getName() + ". Please check " << error_code_config_path_); -#endif - } - diag_pub_ = nh_.advertise(ros::this_node::getName() + "/diag", 10); - boost::thread rate_check_thread(boost::bind(&diag_manager::check_rate_, this));; - enable_diag_ = true; -} - -diag_manager::~diag_manager() -{ - -} - -void diag_manager::check_rate_() -{ - ros::Rate rate(RATE_CHECK_FREQUENCY); - while(ros::ok()) - { - for (auto const& checker : checkers_) - { - boost::optional rate = checker.second->get_rate(); - boost::optional info = query_diag_info(checker.first); - if(rate && info && info.get().threshold) - { - if(*rate required_error_code = {LOW_RELIABILITY}; - if(check_error_code(num, required_error_code)) - { - boost::optional info = query_diag_info(num); - ADD_DIAG_LOG_WARN(info.get().description); - publish_diag_(query_diag_info(info.get().num).get()); - } - return; -} - -void diag_manager::DIAG_RATE_CHECK(int num) -{ - if(enable_diag_ == false) - return; - std::vector required_error_code = {LOW_SUBSCRIBE_RATE, LOW_PUBLISH_RATE, LOW_OPERATION_CYCLE}; - if(check_error_code(num, required_error_code)) - { - try - { - checkers_.at(num)->check(); - } - catch(std::out_of_range&) - { -#ifdef STRICT_ERROR_CODE_CHECK - ROS_ERROR_STREAM("rate checker object cannot found. Please check " << error_code_config_path_); - WRITE_LOG(); - std::exit(-1); -#else - ROS_WARN_STREAM("rate checker object cannot found. Please check " << error_code_config_path_); -#endif - } - } - return; -} - -boost::optional diag_manager::query_diag_info(int num) -{ - for(auto diag_info_itr = diag_info_.begin(); diag_info_itr != diag_info_.end(); diag_info_itr++) - { - if(diag_info_itr->num == num) - { - if(diag_info_itr->threshold && diag_info_itr->level) - { - diag_info ret(diag_info_itr->num, diag_info_itr->name, diag_info_itr->category, diag_info_itr->description, diag_info_itr->threshold.get(), diag_info_itr->level.get()); - return ret; - } - else - { - diag_info ret(diag_info_itr->num, diag_info_itr->name, diag_info_itr->category, diag_info_itr->description); - return ret; - } - } - } - return boost::none; -} - -void diag_manager::WRITE_LOG() -{ - namespace fs = boost::filesystem; - const fs::path path("/tmp/Autoware/Diag/Log/" + ros::this_node::getName()); - boost::system::error_code error; - const bool result = fs::create_directories(path, error); - std::vector::iterator it = diag_log_.begin(); - std::string write_string = ""; - while( it != diag_log_.end() ) { - write_string = write_string + *it + "\n"; - ++it; - } - std::ofstream outputfile(std::string("/tmp/Autoware/Diag/Log/" + ros::this_node::getName() + "/log.txt").c_str()); - outputfile << write_string; - outputfile.close(); - return; -} - -void diag_manager::ADD_DIAG_LOG_ERROR(std::string log_text) -{ - log_text = "in " + ros::this_node::getName() + ": " + log_text; - boost::posix_time::ptime my_posix_time = ros::Time::now().toBoost(); - std::string iso_time_str = boost::posix_time::to_iso_extended_string(my_posix_time); - std::string text = "[" + iso_time_str + "] : " + log_text; - ROS_ERROR_STREAM(log_text); - diag_log_.push_back(text); - return; -} - -void diag_manager::ADD_DIAG_LOG_WARN(std::string log_text) -{ - log_text = "in " + ros::this_node::getName() + ": " + log_text; - boost::posix_time::ptime my_posix_time = ros::Time::now().toBoost(); - std::string iso_time_str = boost::posix_time::to_iso_extended_string(my_posix_time); - std::string text = "[" + iso_time_str + "] : " + log_text; - ROS_WARN_STREAM(log_text); - diag_log_.push_back(text); - return; -} - -void diag_manager::DIAG_RESOURCE(std::string target_resource_path, int num) -{ - if(enable_diag_ == false) - return; - std::vector required_error_code = {RESOURCE_NOT_FOUND}; - if(check_error_code(num, required_error_code)) - { - if(query_diag_info(num)) - { - namespace fs = boost::filesystem; - fs::path path(target_resource_path); - boost::system::error_code error; - const bool result = fs::exists(path, error); - if (!result || error) - { - //ROS_ERROR_STREAM("required resource " << path << " does not found."); - ADD_DIAG_LOG_ERROR("required resource " + target_resource_path + " does not found."); - publish_diag_(query_diag_info(num).get()); - ROS_ERROR_STREAM("shutdonw " << ros::this_node::getName()); - WRITE_LOG(); - std::exit(-1); - } - return; - } - else - { - ADD_DIAG_LOG_WARN(query_diag_info(num)->description); - publish_diag_(query_diag_info(num).get()); - return; - } - } -} - -bool diag_manager::diag_resource(std::string target_resource_path) -{ - namespace fs = boost::filesystem; - fs::path path(target_resource_path); - boost::system::error_code error; - const bool result = fs::exists(path, error); - if (!result || error) - { - ADD_DIAG_LOG_ERROR("required resource " + target_resource_path + " does not found."); - ADD_DIAG_LOG_ERROR("disable diag module in " + ros::this_node::getName()); - return false; - } - return true; -} - -bool diag_manager::check_error_code(int requested_error_number, std::vector right_categories) -{ - if(query_diag_info(requested_error_number)) - { - for(auto category_itr = right_categories.begin(); category_itr != right_categories.end(); category_itr++) - { - if(*category_itr == query_diag_info(requested_error_number)->category) - { - return true; - } - } -#ifdef STRICT_ERROR_CODE_CHECK - ROS_ERROR_STREAM("error category : " << query_diag_info(requested_error_number)->category << " in " << ros::this_node::getName() << " does not match. Please check " << error_code_config_path_); - WRITE_LOG(); - std::exit(-1); -#else - ROS_WARN_STREAM("error category : " << query_diag_info(requested_error_number)->category << " in " << ros::this_node::getName() << " does not match. Please check " << error_code_config_path_); -#endif - return false; - } - else - { - ROS_WARN_STREAM("error number : " << requested_error_number << " in " << ros::this_node::getName() << " does not exist. Check " << error_code_config_path_); - return false; - } -} - -void diag_manager::publish_diag_(diag_info info) -{ - diag_msgs::diag_error msg; - msg.num = info.num; - msg.name = info.name; - msg.category = info.category; - msg.description = info.description; - diag_pub_.publish(msg); - return; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_subscriber.cpp b/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_subscriber.cpp deleted file mode 100644 index 01e68ae551a..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/src/diag_subscriber.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include - -diag_subscriber::diag_subscriber(std::string target_node,int target_node_number) : target_node_(target_node), target_node_number_(target_node_number) -{ - diag_sub_ = nh_.subscribe(target_node+"/diag", 1, &diag_subscriber::callback_, this); -} - -diag_subscriber::~diag_subscriber() -{ - -} - -void diag_subscriber::callback_(diag_msgs::diag_error msg) -{ - mtx_.lock(); - buffer_.push_back(msg); - //ROS_ERROR_STREAM(msg); - mtx_.unlock(); - return; -} - -diag_msgs::diag_node_errors diag_subscriber::get_diag_node_errors() -{ - diag_msgs::diag_node_errors ret; - mtx_.lock(); - std::copy(buffer_.begin(), buffer_.end(), back_inserter(ret.errors)); - buffer_.clear(); - mtx_.unlock(); - ret.header.stamp = ros::Time::now(); - ret.node_number = target_node_number_; - return ret; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/src/rate_checker.cpp b/ros/src/common/libs/diagnostics_lib/diag_lib/src/rate_checker.cpp deleted file mode 100644 index 55025fd0896..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/src/rate_checker.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include - -rate_checker::rate_checker(double buffer_length) : buffer_length_(buffer_length) -{ - start_time_ = ros::Time::now(); -} - -rate_checker::~rate_checker() -{ - -} - -void rate_checker::check() -{ - update_(); - mtx_.lock(); - data_.push_back(ros::Time::now()); - mtx_.unlock(); -} - -void rate_checker::update_() -{ - std::vector buffer; - for(auto data_itr = data_.begin(); data_itr != data_.end(); data_itr++) - { - if(*data_itr > ros::Time::now()-ros::Duration(buffer_length_)) - { - buffer.push_back(*data_itr); - } - } - mtx_.lock(); - data_ = buffer; - mtx_.unlock(); - return; -} - -boost::optional rate_checker::get_rate() -{ - boost::optional rate; - if(ros::Time::now() - start_time_ < ros::Duration(buffer_length_)) - { - return boost::none; - } - update_(); - mtx_.lock(); - rate = data_.size()/buffer_length_; - mtx_.unlock(); - return rate; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog.cpp b/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog.cpp deleted file mode 100644 index 9f59279d432..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog.cpp +++ /dev/null @@ -1,144 +0,0 @@ -//headers in diag_lib -#include - -//headers in ROS -#include - -watchdog::watchdog() -{ - nh_.param("/error_code_config_path", config_filepath_, ""); - nh_.param(ros::this_node::getName() + "/publish_rate", publish_rate_, 10); - diag_.DIAG_RESOURCE(config_filepath_, 0); - YAML::Node config = YAML::LoadFile(config_filepath_.c_str()); - for(YAML::const_iterator it=config.begin();it != config.end();++it) - { - if(it->first.as() != ros::this_node::getName()) - { - std::string itr_node_name = it->first.as(); - diag_target_nodes_.push_back(itr_node_name); - YAML::Node target_node = config[itr_node_name]; - diag_sub_[itr_node_name] = boost::make_shared(itr_node_name, target_node["node_number"].as()); - boost::shared_ptr > target_node_watchdog_diag_info = boost::make_shared >(); - for(const YAML::Node &error : target_node["errors"]) - { - if(error["category"].as() == NODE_IS_DEAD) - { - diag_info info(error["num"].as(), error["name"].as(), error["category"].as(), error["description"].as()); - target_node_watchdog_diag_info->push_back(info); - } - } - if(target_node_watchdog_diag_info->size() != 0) - { - watchdog_diag_info_[itr_node_name] = target_node_watchdog_diag_info; - watchdog_target_nodes_.push_back(itr_node_name); - } - } - } - write_error_code_csv_(config); - diag_pub_ = nh_.advertise(ros::this_node::getName()+"/diag/all", 1); -} - -watchdog::~watchdog() -{ - -} - -void watchdog::write_error_code_csv_(YAML::Node config) -{ - namespace fs = boost::filesystem; - const fs::path path("/tmp/Autoware/Diag/"); - boost::system::error_code error; - const bool result = fs::create_directories(path, error); - std::string write_string = "node_name,node_number,num,name,category,description,threshold,level\n"; - for(YAML::const_iterator it=config.begin();it != config.end();++it) - { - std::string node_name = it->first.as(); - YAML::Node target_node = config[node_name]; - int node_number = target_node["node_number"].as(); - for(const YAML::Node &error : target_node["errors"]) - { - std::string line = ""; - int num = error["num"].as(); - std::string name = error["name"].as(); - int category = error["category"].as(); - std::string description = error["description"].as(); - if((category == LOW_SUBSCRIBE_RATE) || (category == LOW_PUBLISH_RATE) || (category == LOW_OPERATION_CYCLE)) - { - double threashold = error["threshold"].as(); - std::string level = error["level"].as(); - line = node_name + "," + std::to_string(node_number) + "," + std::to_string(num) + "," + name + "," + std::to_string(category) + "," + description + "," + std::to_string(threashold) + "," + level + "\n"; - } - else - { - line = node_name + "," + std::to_string(node_number) + "," + std::to_string(num) + "," + name + "," + std::to_string(category) + "," + description + ",,\n"; - } - write_string = write_string + line; - } - } - std::ofstream outputfile(std::string("/tmp/Autoware/Diag/error_list.csv").c_str()); - outputfile << write_string; - outputfile.close(); - return; -} - -void watchdog::publish_diag_() -{ - ros::Rate rate(publish_rate_); - while(ros::ok()) - { - update_connection_status_(); - diag_msgs::diag diag_msg; - for(auto itr = watchdog_target_nodes_.begin(); itr != watchdog_target_nodes_.end(); ++itr) - { - diag_msgs::diag_node_errors errors = diag_sub_[*itr]->get_diag_node_errors(); - if(connection_status_[*itr] == false) - { - std::vector target_node_watchdog_diag_info = *watchdog_diag_info_[*itr]; - for(int i=0; i detected_nodes; - ros::master::getNodes(detected_nodes); - for(auto itr = watchdog_target_nodes_.begin(); itr != watchdog_target_nodes_.end(); ++itr) - { - if(std::find(detected_nodes.begin(), detected_nodes.end(), *itr) != detected_nodes.end()) - { - connection_status_[*itr] = true; - } - else - { - connection_status_[*itr] = false; - } - } - return; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog_node.cpp b/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog_node.cpp deleted file mode 100644 index 1e13693d2d0..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_lib/src/watchdog_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -#include - -int main(int argc, char *argv[]) { - ros::init(argc, argv, "watchdog_node"); - watchdog wd; - wd.run(); - return 0; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/CHANGELOG.rst b/ros/src/common/libs/diagnostics_lib/diag_msgs/CHANGELOG.rst deleted file mode 100644 index 7a164831277..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/CHANGELOG.rst +++ /dev/null @@ -1,132 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package state -^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.10.0 (2019-01-17) -------------------- - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- -* add disable mode for diag_manager -* add diag_lib - add csv writer - remove unused comment - fix maintainer names - fix email address - fix dependency - fix buildtool_depend, remove unused lines from find_package -* Contributors: Masaya Kataoka - -1.6.3 (2018-03-06) ------------------- - -1.6.2 (2018-02-27) ------------------- -* Update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.1 (2018-01-20) ------------------- -* update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.0 (2017-12-11) ------------------- -* Prepare release for 1.6.0 -* add new state for lanechange -* add new state for all stopline pause -* Checked coding by cppcheck and apply clang-format -* Add new state - - TrafficLight State (it will be planning to change "behavior" to - another category) - - Crawl(slow speed) -* Add to support multiple lane shift -* Fixed: - - callback - - laneshift - Added: - - publisher for laneid - - new lanechange flag - - new param for decisionMaker -* add to able to have multiple-state -* change way to state management -* cosme -* Fix not working changed callback -* delete build warning, and change stopline -* update state and remove detection state -* Fix a state changing bug -* add support to stopline -* fix segv -* add state changed callback -* minor fixes -* Add feature of to find stopline. and following minor fixes - - to change vectormap operation to vectormap lib. - - to change state operation -* Support to lanechange similar to state_machine(old) package -* :put_litter_in_its_place: -* Add support to switch on/off directional indicator -* remove unnecessary code from decisionmaker -* add support to waypoint velocity control by state -* update decisionmaker and related library - - add multiplelane path recognition - - renamed euc -* Add to support manually decision -* Contributors: Yamato ANDO, Yusuke FUJII - -1.5.1 (2017-09-25) ------------------- -* Release/1.5.1 (`#816 `_) - * fix a build error by gcc version - * fix build error for older indigo version - * update changelog for v1.5.1 - * 1.5.1 -* Contributors: Yusuke FUJII - -1.5.0 (2017-09-21) ------------------- -* Update changelog -* apply clang-format -* Add to support dynamical parameter for decision_maker -* Fix a build error for libstate -* Move the decision part of the state machine library to decision_Maker node. This is WIP. -* Fixed configuration of state -* apply clang-format -* organize package files and directories -* Add a decision_maker package - The decision_maker package determines the intention of what actions the - local planner and control node should take based on perception nodes, - global planner nodes, map data, sensor data. - This commit corresponds only to the following functions. - - Behavior state recognition - - Dynamic selection of local planner (It is necessary to change the topic name of local planner) -* Contributors: Yusuke FUJII - -1.4.0 (2017-08-04) ------------------- - -1.3.1 (2017-07-16) ------------------- - -1.3.0 (2017-07-14) ------------------- - -1.2.0 (2017-06-07) ------------------- - -1.1.2 (2017-02-27 23:10) ------------------------- - -1.1.1 (2017-02-27 22:25) ------------------------- - -1.1.0 (2017-02-24) ------------------- - -1.0.1 (2017-01-14) ------------------- - -1.0.0 (2016-12-22) ------------------- diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag.msg b/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag.msg deleted file mode 100644 index 68349297069..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag.msg +++ /dev/null @@ -1,6 +0,0 @@ -#message for all diagnostics in the systems - -#header for timestamp -Header header -#list of errors in the target nodes -diag_node_errors[] nodes diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_error.msg b/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_error.msg deleted file mode 100644 index 080e76ed032..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_error.msg +++ /dev/null @@ -1,10 +0,0 @@ -#the message which describes a single error - -#name of the error -string name -#error number in the target node -int32 num -#error category -int32 category -#desctiption of the error -string description diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_node_errors.msg b/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_node_errors.msg deleted file mode 100644 index a981b9b6208..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/msg/diag_node_errors.msg +++ /dev/null @@ -1,8 +0,0 @@ -#the message which describes errors in a single node - -#header for timestamp -Header header -#number of the target node -int32 node_number -#list of error messages in the target node -diag_error[] errors diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/package.xml b/ros/src/common/libs/diagnostics_lib/diag_msgs/package.xml deleted file mode 100644 index c4f42825413..00000000000 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - diag_msgs - 1.10.0 - The libdiag package - - Masaya Kataoka - Apache 2 - Yusuke FUJII - Masaya Kataoka - catkin - autoware_build_flags - std_msgs - message_generation - message_runtime - std_msgs - message_generation - message_runtime - - - diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CHANGELOG.rst b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CHANGELOG.rst deleted file mode 100644 index 8d27620d8bd..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CHANGELOG.rst +++ /dev/null @@ -1,25 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package fake_autoware_nodes -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.10.0 (2019-01-17) -------------------- - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- -* add filter to the fake_autoware_node -* dd diag_filter to the fake_subscriber -* add diag_lib - add csv writer - remove unused comment - fix maintainer names - fix email address - fix dependency - fix buildtool_depend, remove unused lines from find_package -* Contributors: Masaya Kataoka - -* add diag_lib -* Contributors: Masaya Kataoka diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CMakeLists.txt b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CMakeLists.txt deleted file mode 100644 index 1c391d148dc..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(fake_autoware_nodes) - -add_compile_options(-std=c++11) - -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp - rospy - sensor_msgs - std_msgs - diag_lib -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs diag_lib -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -set(fake_publisher_node_src src/fake_publisher_node.cpp src/fake_publisher.cpp) -add_executable(fake_publisher_node ${fake_publisher_node_src}) -target_link_libraries(fake_publisher_node ${catkin_LIBRARIES}) -add_dependencies(fake_publisher_node diag_msgs_generate_messages_cpp) - -set(fake_subscriber_node_src src/fake_subscriber_node.cpp src/fake_subscriber.cpp) -add_executable(fake_subscriber_node ${fake_subscriber_node_src}) -target_link_libraries(fake_subscriber_node ${catkin_LIBRARIES}) -add_dependencies(fake_publisher_node diag_msgs_generate_messages_cpp) - -foreach(dir launch) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) - -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_publisher.h b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_publisher.h deleted file mode 100644 index 5a710c76a4c..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_publisher.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef FAKE_PUBLISHER_H_INCLUDED -#define FAKE_PUBLISHER_H_INCLUDED - -//headers in diag_lib -#include - -class fake_publisher -{ - public: - fake_publisher(); - ~fake_publisher(); - void run(); - double divide(double a, double b); - private: - ros::NodeHandle nh_; - ros::Publisher fake_pub_; - diag_manager diag_manager_; -}; - -#endif //FAKE_PUBLISHER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_subscriber.h b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_subscriber.h deleted file mode 100644 index 278bcb28702..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/include/fake_autoware_nodes/fake_subscriber.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef FAKE_SUBSCRIBER_H_INCLUDED -#define FAKE_SUBSCRIBER_H_INCLUDED - -//headers in diag_lib -#include -#include - -//headers in diag_msg -#include -#include - -//headers in std_msgs -#include - -//headers in ROS -#include - -class fake_subscriber -{ - public: - fake_subscriber(); - ~fake_subscriber(); - private: - void diag_callback_(const diag_msgs::diagConstPtr msg); - void callback_(const std_msgs::Float64ConstPtr msg); - ros::NodeHandle nh_; - ros::Subscriber fake_sub_; - ros::Subscriber diag_sub_; - diag_manager diag_manager_; - diag_filter diag_filter_; -}; - -#endif //FAKE_SUBSCRIBER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/diag_sample.launch b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/diag_sample.launch deleted file mode 100644 index 5677fed2133..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/diag_sample.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/fake_autoware_nodes.launch b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/fake_autoware_nodes.launch deleted file mode 100644 index 3db9981d35d..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/launch/fake_autoware_nodes.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/package.xml b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/package.xml deleted file mode 100644 index d9e8739d59d..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - fake_autoware_nodes - 1.10.0 - The fake_autoware_nodes package - - Masaya Kataoka - - Apache 2 - - Masaya Kataoka - - catkin - autoware_build_flags - geometry_msgs - roscpp - rospy - sensor_msgs - std_msgs - diag_lib - geometry_msgs - roscpp - rospy - sensor_msgs - std_msgs - diag_lib - geometry_msgs - roscpp - rospy - sensor_msgs - std_msgs - diag_lib - - - - diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher.cpp b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher.cpp deleted file mode 100644 index e148e9c9275..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include - -#include - -fake_publisher::fake_publisher() -{ - fake_pub_ = nh_.advertise(ros::this_node::getName()+"/data", 1); -} - -fake_publisher::~fake_publisher() -{ - -} - -void fake_publisher::run() -{ - ros::Rate rate(10); - std_msgs::Float64 msg; - msg.data = -50; - while(ros::ok()) - { - diag_manager_.DIAG_RATE_CHECK(3); - fake_pub_.publish(msg); - msg.data = msg.data + 1.0; - diag_manager_.DIAG_ASSERT_VALUE_RANGE(-20.0, 20.0, msg.data, 0); - if(msg.data == 50) - { - diag_manager_.DIAG_LOW_RELIABILITY(4); - msg.data = -50; - } - try - { - divide(1,msg.data); - } - catch(std::range_error& exception) - { - diag_manager_.DIAG_ASSERT_EXCEPTION(exception,1); - } - rate.sleep(); - } - diag_manager_.WRITE_LOG(); - return; -} - -double fake_publisher::divide(double a, double b) -{ - if (b == 0) - { - throw std::range_error("Divided by zero."); - } - return a / b; -} diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher_node.cpp b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher_node.cpp deleted file mode 100644 index abc3c37b5dd..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_publisher_node.cpp +++ /dev/null @@ -1,12 +0,0 @@ -//headers in ROS -#include - -//headers in fake_autoware_nodes -#include - -int main(int argc, char *argv[]) { - ros::init(argc, argv, "fake_publisher_node"); - fake_publisher fake_pub; - fake_pub.run(); - return 0; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber.cpp b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber.cpp deleted file mode 100644 index aa18acef14e..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include - -fake_subscriber::fake_subscriber() -{ - diag_sub_ = nh_.subscribe("watchdog_node/diag/all", 1, &fake_subscriber::diag_callback_, this); - fake_sub_ = nh_.subscribe(ros::this_node::getName()+"/data", 1, &fake_subscriber::callback_, this); -} - -fake_subscriber::~fake_subscriber() -{ - -} - -void fake_subscriber::callback_(const std_msgs::Float64ConstPtr msg) -{ - diag_manager_.DIAG_ASSERT_VALUE_MIN(0.0, msg->data, 0); - return; -} - -void fake_subscriber::diag_callback_(const diag_msgs::diagConstPtr msg) -{ - boost::optional error = diag_filter_.filter(*msg, ros::this_node::getName()); - if(error) - { - ROS_ERROR_STREAM(error.get()); - } - return; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber_node.cpp b/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber_node.cpp deleted file mode 100644 index 244582ace61..00000000000 --- a/ros/src/common/libs/diagnostics_lib/fake_autoware_nodes/src/fake_subscriber_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include - -//headers in diag_lib -#include - -//headers in fake_autoware_nodes -#include - -int main(int argc, char *argv[]) { - ros::init(argc, argv, "fake_subscriber_node"); - fake_subscriber fake_sub; - ros::spin(); - return 0; -} \ No newline at end of file diff --git a/ros/src/common/libs/diagnostics_lib/rosgraph.png b/ros/src/common/libs/diagnostics_lib/rosgraph.png deleted file mode 100644 index ee8d53b9f86..00000000000 Binary files a/ros/src/common/libs/diagnostics_lib/rosgraph.png and /dev/null differ diff --git a/ros/src/computing/perception/localization/packages/lidar_localizer/CMakeLists.txt b/ros/src/computing/perception/localization/packages/lidar_localizer/CMakeLists.txt index d72fd69eb2c..89cf5311dc8 100644 --- a/ros/src/computing/perception/localization/packages/lidar_localizer/CMakeLists.txt +++ b/ros/src/computing/perception/localization/packages/lidar_localizer/CMakeLists.txt @@ -50,6 +50,7 @@ find_package(catkin REQUIRED COMPONENTS velodyne_pointcloud ndt_tku ndt_cpu + autoware_health_checker ${PCL_OPENMP_PACKAGES} autoware_msgs autoware_config_msgs diff --git a/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp index 8e1613d5e32..4ae31227324 100644 --- a/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp +++ b/ros/src/computing/perception/localization/packages/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp @@ -69,12 +69,17 @@ #include +//headers in Autoware Health Checker +#include + #define PREDICT_POSE_THRESHOLD 0.5 #define Wa 0.4 #define Wb 0.3 #define Wc 0.3 +static std::shared_ptr node_status_publisher_ptr_; + struct pose { double x; @@ -916,6 +921,7 @@ static void imu_callback(const sensor_msgs::Imu::Ptr& input) static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/points_raw/slow",8,5,1,"topic points_raw subscribe rate low."); if (map_loaded == 1 && init_pos_set == 1) { matching_start = std::chrono::system_clock::now(); @@ -1348,6 +1354,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) } predict_pose_pub.publish(predict_pose_msg); + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/ndt_pose/slow",8,5,1,"topic points_raw publish rate low."); ndt_pose_pub.publish(ndt_pose_msg); // current_pose is published by vel_pose_mux // current_pose_pub.publish(current_pose_msg); @@ -1369,6 +1376,7 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) matching_end = std::chrono::system_clock::now(); exe_time = std::chrono::duration_cast(matching_end - matching_start).count() / 1000.0; time_ndt_matching.data = exe_time; + node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/time_ndt_matching",time_ndt_matching.data,50,70,100,"value time_ndt_matching is too high."); time_ndt_matching_pub.publish(time_ndt_matching); // Set values for /estimate_twist @@ -1386,6 +1394,8 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) geometry_msgs::Vector3Stamped estimate_vel_msg; estimate_vel_msg.header.stamp = current_scan_time; estimate_vel_msg.vector.x = current_velocity; + node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/estimate_twist/linear",current_velocity,5,10,15,"value linear estimated twist is too high."); + node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/estimate_twist/angular",angular_velocity,5,10,15,"value linear angular twist is too high."); estimated_vel_pub.publish(estimate_vel_msg); // Set values for /ndt_stat @@ -1514,6 +1524,9 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle private_nh("~"); + node_status_publisher_ptr_ = std::make_shared(nh,private_nh); + node_status_publisher_ptr_->ENABLE(); + node_status_publisher_ptr_->NODE_ACTIVATE(); // Set log file name. private_nh.getParam("output_log_data", _output_log_data); diff --git a/ros/src/computing/perception/localization/packages/lidar_localizer/package.xml b/ros/src/computing/perception/localization/packages/lidar_localizer/package.xml index 7d52101202a..56c51731a0e 100644 --- a/ros/src/computing/perception/localization/packages/lidar_localizer/package.xml +++ b/ros/src/computing/perception/localization/packages/lidar_localizer/package.xml @@ -25,6 +25,7 @@ ndt_tku libpcl-all-dev eigen + autoware_health_checker roscpp std_msgs @@ -43,6 +44,7 @@ ndt_tku libpcl-all eigen + autoware_health_checker diff --git a/ros/src/computing/planning/motion/packages/astar_planner/CMakeLists.txt b/ros/src/computing/planning/motion/packages/astar_planner/CMakeLists.txt index c9978c77545..58c3f7ff729 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/CMakeLists.txt +++ b/ros/src/computing/planning/motion/packages/astar_planner/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS tf waypoint_follower autoware_msgs + autoware_health_checker ) ################################### diff --git a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp index 0680e9fef89..3e85174f357 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp +++ b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.cpp @@ -30,7 +30,10 @@ SearchInfo::SearchInfo() , state_("") , upper_bound_distance_(-1) { + ros::NodeHandle nh; ros::NodeHandle private_nh_("~"); + node_status_publisher_ptr_ = std::make_shared(nh,private_nh_); + node_status_publisher_ptr_->ENABLE(); private_nh_.param("map_frame", map_frame_, "map"); private_nh_.param("obstacle_detect_count", obstacle_detect_count_, 10); private_nh_.param("avoid_distance", avoid_distance_, 13); @@ -95,7 +98,15 @@ void SearchInfo::mapCallback(const nav_msgs::OccupancyGridConstPtr &msg) void SearchInfo::currentPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg) { current_pose_ = *msg; - + node_status_publisher_ptr_->NODE_ACTIVATE(); + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/current_pose/slow",8,5,1,"topic current_pose subscribe rate low."); + if(closest_waypoint_index_!=-1 && path_set_) + { + autoware_msgs::Waypoint closest_waypoint = subscribed_waypoints_.waypoints[closest_waypoint_index_]; + double dist = std::sqrt(std::pow(closest_waypoint.pose.pose.position.x-current_pose_.pose.position.x,2) + +std::pow(closest_waypoint.pose.pose.position.y-current_pose_.pose.position.y,2)); + node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/range/closest_waypoint_distance",dist,0.5,1.0,2.0,"distance between closest_waypoint and current_pose is too long."); + } return; } @@ -171,6 +182,7 @@ void SearchInfo::waypointsCallback(const autoware_msgs::LaneConstPtr &msg) void SearchInfo::closestWaypointCallback(const std_msgs::Int32ConstPtr &msg) { + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/closest_waypoint/slow",8,5,1,"topic closest_waypoint subscribe rate low."); closest_waypoint_index_ = msg->data; } diff --git a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h index 090081adcf5..4a0d0af0c4a 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h +++ b/ros/src/computing/planning/motion/packages/astar_planner/nodes/obstacle_avoid/search_info_ros.h @@ -27,6 +27,9 @@ #include #include +#include +#include + namespace astar_planner { class SearchInfo @@ -140,7 +143,7 @@ class SearchInfo private: double calcPathLength(const autoware_msgs::Lane &lane, const int start_waypoint_index, const int goal_waypoint_index) const; - + std::shared_ptr node_status_publisher_ptr_; nav_msgs::OccupancyGrid map_; geometry_msgs::PoseStamped start_pose_global_; geometry_msgs::PoseStamped goal_pose_global_; diff --git a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp index 1c6c4901881..42c54190486 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp +++ b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.cpp @@ -41,7 +41,10 @@ VelocitySetInfo::VelocitySetInfo() wpidx_detectionResultByOtherNodes_(-1) { ros::NodeHandle private_nh_("~"); + ros::NodeHandle nh; private_nh_.param("remove_points_upto", remove_points_upto_, 2.3); + node_status_publisher_ptr_ = std::make_shared(nh,private_nh_); + node_status_publisher_ptr_->ENABLE(); } VelocitySetInfo::~VelocitySetInfo() @@ -70,6 +73,7 @@ void VelocitySetInfo::configCallback(const autoware_config_msgs::ConfigVelocityS void VelocitySetInfo::pointsCallback(const sensor_msgs::PointCloud2ConstPtr &msg) { + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/points_no_ground/slow",8,5,1,"topic points_no_ground subscribe rate low."); pcl::PointCloud sub_points; pcl::fromROSMsg(*msg, sub_points); @@ -111,6 +115,7 @@ void VelocitySetInfo::controlPoseCallback(const geometry_msgs::PoseStampedConstP void VelocitySetInfo::localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg) { + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/current_pose/slow",8,5,1,"topic current_pose subscribe rate low."); localizer_pose_ = *msg; } diff --git a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h index 9d0104657fa..13afcc35c76 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h +++ b/ros/src/computing/planning/motion/packages/astar_planner/nodes/velocity_set/velocity_set_info.h @@ -24,6 +24,9 @@ #include "autoware_config_msgs/ConfigVelocitySet.h" +#include +#include + class VelocitySetInfo { private: @@ -51,6 +54,8 @@ class VelocitySetInfo bool set_pose_; bool use_obstacle_sim_; + std::shared_ptr node_status_publisher_ptr_; + public: VelocitySetInfo(); ~VelocitySetInfo(); diff --git a/ros/src/computing/planning/motion/packages/astar_planner/package.xml b/ros/src/computing/planning/motion/packages/astar_planner/package.xml index 73d4016a0b2..a351946f6f4 100644 --- a/ros/src/computing/planning/motion/packages/astar_planner/package.xml +++ b/ros/src/computing/planning/motion/packages/astar_planner/package.xml @@ -16,6 +16,7 @@ std_msgs tf vector_map + autoware_health_checker roscpp std_msgs @@ -23,6 +24,7 @@ waypoint_follower autoware_msgs vector_map + autoware_health_checker diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt index 70f43ed7c38..22a26efe0b6 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS autoware_msgs autoware_config_msgs tablet_socket_msgs + autoware_health_checker ) ################################################ @@ -42,6 +43,7 @@ catkin_package( pcl_conversions sensor_msgs tablet_socket_msgs + autoware_health_checker ) ########### diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp index 804398a13ae..7ba4976ee86 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp @@ -28,6 +28,7 @@ PurePursuit::PurePursuit() , current_linear_velocity_(0) , minimum_lookahead_distance_(6) { + } // Destructor diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp index 2a08a7294ca..edbdfc1e8fc 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.cpp @@ -36,7 +36,8 @@ PurePursuitNode::PurePursuitNode() , minimum_lookahead_distance_(6.0) { initForROS(); - + node_status_publisher_ptr_ = std::make_shared(nh_,private_nh_); + node_status_publisher_ptr_->ENABLE(); // initialize for PurePursuit pp_.setLinearInterpolationParameter(is_linear_interpolation_); } @@ -92,9 +93,11 @@ void PurePursuitNode::run() double kappa = 0; bool can_get_curvature = pp_.canGetCurvature(&kappa); + publishTwistStamped(can_get_curvature, kappa); publishControlCommandStamped(can_get_curvature, kappa); - + node_status_publisher_ptr_->NODE_ACTIVATE(); + node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topic vehicle_cmd publish rate low."); // for visualization with Rviz pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); pub13_.publish(displaySearchRadius(pp_.getCurrentPose().position, pp_.getLookaheadDistance())); @@ -121,6 +124,7 @@ void PurePursuitNode::publishTwistStamped(const bool &can_get_curvature, const d ts.header.stamp = ros::Time::now(); ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0; ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0; + node_status_publisher_ptr_->CHECK_MAX_VALUE("/value/twist",ts.twist.linear.x,2.2,3.3,4.4,"linear twist_cmd is too high"); pub1_.publish(ts); } diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h index 2d1a69961bf..627851457c9 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit_core.h @@ -31,6 +31,10 @@ #include "pure_pursuit.h" #include "pure_pursuit_viz.h" +#include + +#include + namespace waypoint_follower { enum class Mode : int32_t @@ -60,6 +64,8 @@ class PurePursuitNode ros::NodeHandle nh_; ros::NodeHandle private_nh_; + std::shared_ptr node_status_publisher_ptr_; + // class PurePursuit pp_; diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp index 70308506e30..341bd4d1731 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_gate/twist_gate.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,9 @@ #include "autoware_msgs/SteerCmd.h" #include "autoware_msgs/ControlCommandStamped.h" +//headers in Autowae Health Checker +#include + class TwistGate { using remote_msgs_t = autoware_msgs::RemoteCmd; @@ -72,6 +76,7 @@ class TwistGate ros::NodeHandle nh_; ros::NodeHandle private_nh_; + std::shared_ptr node_status_pub_ptr_; ros::Publisher emergency_stop_pub_; ros::Publisher control_command_pub_; ros::Publisher vehicle_cmd_pub_; @@ -99,6 +104,7 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n ,command_mode_(CommandMode::AUTO) ,previous_command_mode_(CommandMode::AUTO) { + node_status_pub_ptr_ = std::make_shared(nh_,private_nh_); emergency_stop_pub_ = nh_.advertise("/emergency_stop", 1, true); control_command_pub_ = nh_.advertise("/ctrl_mode", 1); vehicle_cmd_pub_ = nh_.advertise("/vehicle_cmd", 1, true); @@ -118,6 +124,7 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n twist_gate_msg_.header.seq = 0; emergency_stop_msg_.data = false; send_emergency_cmd = false; + node_status_pub_ptr_->ENABLE(); remote_cmd_time_ = ros::Time::now(); watchdog_timer_thread_ = std::thread(&TwistGate::watchdog_timer, this); @@ -226,6 +233,8 @@ void TwistGate::remote_cmd_callback(const remote_msgs_t::ConstPtr& input_msg) void TwistGate::auto_cmd_twist_cmd_callback(const geometry_msgs::TwistStamped::ConstPtr& input_msg) { + node_status_pub_ptr_->NODE_ACTIVATE(); + node_status_pub_ptr_->CHECK_RATE("/topic/rate/twist_cmd/slow",8,5,1,"topic twist_cmd subscribe rate low."); if(command_mode_ == CommandMode::AUTO) { twist_gate_msg_.header.frame_id = input_msg->header.frame_id; diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/package.xml b/ros/src/computing/planning/motion/packages/waypoint_follower/package.xml index 19bfc70bc9d..a937b85dfc4 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/package.xml +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/package.xml @@ -18,6 +18,7 @@ pcl_conversions sensor_msgs tablet_socket_msgs + autoware_health_checker roscpp std_msgs @@ -29,7 +30,7 @@ pcl_conversions sensor_msgs tablet_socket_msgs - + autoware_health_checker diff --git a/ros/src/common/libs/diagnostics_lib/diag_msgs/CMakeLists.txt b/ros/src/msgs/autoware_system_msgs/CMakeLists.txt similarity index 58% rename from ros/src/common/libs/diagnostics_lib/diag_msgs/CMakeLists.txt rename to ros/src/msgs/autoware_system_msgs/CMakeLists.txt index b25afb2d0bf..6f242891aae 100644 --- a/ros/src/common/libs/diagnostics_lib/diag_msgs/CMakeLists.txt +++ b/ros/src/msgs/autoware_system_msgs/CMakeLists.txt @@ -1,28 +1,30 @@ cmake_minimum_required(VERSION 2.8.3) -project(diag_msgs) - +project(autoware_system_msgs) +add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS message_generation std_msgs - message_runtime ) add_message_files( FILES - diag_error.msg - diag_node_errors.msg - diag.msg - ) + DiagnosticStatus.msg + DiagnosticStatusArray.msg + SystemStatus.msg + NodeStatus.msg + HardwareStatus.msg +) -generate_messages( - DEPENDENCIES - std_msgs) +generate_messages(DEPENDENCIES + std_msgs +) catkin_package( CATKIN_DEPENDS - std_msgs message_runtime + message_runtime + std_msgs ) foreach(dir msg) diff --git a/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg new file mode 100755 index 00000000000..4abe2e1489d --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatus.msg @@ -0,0 +1,20 @@ +Header header + +string key + +string value + +string description + +uint8 UNDEFINED = 0 + +uint8 type +uint8 OUT_OF_RANGE = 1 +uint8 RATE_IS_SLOW = 2 +uint8 HARDWARE = 255 + +uint8 level +uint8 OK = 1 +uint8 WARN = 2 +uint8 ERROR = 3 +uint8 FATAL = 4 \ No newline at end of file diff --git a/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatusArray.msg b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatusArray.msg new file mode 100755 index 00000000000..f0ea4636beb --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/msg/DiagnosticStatusArray.msg @@ -0,0 +1 @@ +autoware_system_msgs/DiagnosticStatus[] status \ No newline at end of file diff --git a/ros/src/msgs/autoware_system_msgs/msg/HardwareStatus.msg b/ros/src/msgs/autoware_system_msgs/msg/HardwareStatus.msg new file mode 100755 index 00000000000..4a408ef8f08 --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/msg/HardwareStatus.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +string hardware_name +autoware_system_msgs/DiagnosticStatusArray[] status \ No newline at end of file diff --git a/ros/src/msgs/autoware_system_msgs/msg/NodeStatus.msg b/ros/src/msgs/autoware_system_msgs/msg/NodeStatus.msg new file mode 100755 index 00000000000..efd420a4108 --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/msg/NodeStatus.msg @@ -0,0 +1,4 @@ +Header header +string node_name +bool node_activated +autoware_system_msgs/DiagnosticStatusArray[] status \ No newline at end of file diff --git a/ros/src/msgs/autoware_system_msgs/msg/SystemStatus.msg b/ros/src/msgs/autoware_system_msgs/msg/SystemStatus.msg new file mode 100755 index 00000000000..44b25921ff2 --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/msg/SystemStatus.msg @@ -0,0 +1,4 @@ +Header header +string[] available_nodes +autoware_system_msgs/NodeStatus[] node_status +autoware_system_msgs/HardwareStatus[] hardware_status \ No newline at end of file diff --git a/ros/src/msgs/autoware_system_msgs/package.xml b/ros/src/msgs/autoware_system_msgs/package.xml new file mode 100644 index 00000000000..2abb87a7fdd --- /dev/null +++ b/ros/src/msgs/autoware_system_msgs/package.xml @@ -0,0 +1,22 @@ + + + autoware_system_msgs + 0.0.0 + The autoware_system_msgs package + + Masaya Kataoka + + BSD + + catkin + message_generation + std_msgs + + std_msgs + + std_msgs + message_runtime + + + + diff --git a/ros/src/sensing/filters/packages/points_preprocessor/CMakeLists.txt b/ros/src/sensing/filters/packages/points_preprocessor/CMakeLists.txt index c7315e3428f..0a56fc04263 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/CMakeLists.txt +++ b/ros/src/sensing/filters/packages/points_preprocessor/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge velodyne_pointcloud tf + autoware_health_checker ) catkin_package(CATKIN_DEPENDS @@ -26,6 +27,7 @@ catkin_package(CATKIN_DEPENDS velodyne_pointcloud autoware_config_msgs tf + autoware_health_checker ) find_package(Qt5Core REQUIRED) diff --git a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h index f4f2a572085..ca195bac9d6 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h +++ b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/include/ray_ground_filter.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -32,6 +33,9 @@ #include #include "autoware_config_msgs/ConfigRayGroundFilter.h" +//headers in Autoware Health Checker +#include + #include #if (CV_MAJOR_VERSION == 3) #include "gencolors.cpp" @@ -42,7 +46,7 @@ class RayGroundFilter { private: - + std::shared_ptr node_status_pub_ptr_; ros::NodeHandle node_handle_; ros::Subscriber points_node_sub_; ros::Subscriber config_node_sub_; diff --git a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp index 250cdefe8aa..3bca79e5793 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +++ b/ros/src/sensing/filters/packages/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp @@ -283,6 +283,8 @@ void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud::Pt void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr &in_sensor_cloud) { + node_status_pub_ptr_->NODE_ACTIVATE(); + node_status_pub_ptr_->CHECK_RATE("/topic/rate/points_raw/slow",8,5,1,"topic points_raw subscribe rate low."); pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr); @@ -327,6 +329,10 @@ void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr &in_s RayGroundFilter::RayGroundFilter():node_handle_("~") { + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + node_status_pub_ptr_ = std::make_shared(nh,pnh); + node_status_pub_ptr_->ENABLE(); } void RayGroundFilter::Run() diff --git a/ros/src/sensing/filters/packages/points_preprocessor/package.xml b/ros/src/sensing/filters/packages/points_preprocessor/package.xml index 27dd2e9e118..c0db40b4428 100644 --- a/ros/src/sensing/filters/packages/points_preprocessor/package.xml +++ b/ros/src/sensing/filters/packages/points_preprocessor/package.xml @@ -27,6 +27,7 @@ rostest gtest yaml-cpp + autoware_health_checker autoware_config_msgs cv_bridge @@ -40,6 +41,7 @@ velodyne_pointcloud libqt5-core yaml-cpp + autoware_health_checker roslaunch rosunit diff --git a/ros/src/simulation/lgsvl_simulator_bridge/config/default.json b/ros/src/simulation/lgsvl_simulator_bridge/config/default.json new file mode 100644 index 00000000000..45692336e2f --- /dev/null +++ b/ros/src/simulation/lgsvl_simulator_bridge/config/default.json @@ -0,0 +1,29 @@ +{ + "bin_type" : "tier4-develop", + "initial_configuration" : { + "map" : "SanFrancisco", + "time_of_day" : 12.0, + "freeze_time_of_day" : true, + "fog_intensity" : 0.0, + "rain_intensity" : 0.0, + "road_wetness" : 0.0, + "enable_traffic" : true, + "enable_pedestrian" : true, + "traffic_density" : 300 + }, + "vehicles" : + [ + { + "type" : "XE_Rigged-autoware", + "address" : "10.254.1.110", + "port" : 9090, + "command_type" : "twist", + "enable_lidar" : true, + "enable_gps" : true, + "enable_main_camera" : true, + "enable_high_quality_rendering" : true, + "position" : {"n" : 4140310.4, "e" : 590681.5, "h" : 10}, + "orientation" : {"r" : 0.0, "p" : 0.0, "y" : 269.9} + } + ] +} \ No newline at end of file diff --git a/ros/src/simulation/lgsvl_simulator_bridge/config/setting.dat b/ros/src/simulation/lgsvl_simulator_bridge/config/setting.dat new file mode 100644 index 00000000000..6dfeba6d925 --- /dev/null +++ b/ros/src/simulation/lgsvl_simulator_bridge/config/setting.dat @@ -0,0 +1,4 @@ +[simulator_params] +config_path=/home/masaya-tier4/Autoware/ros/src/simulation/lgsvl_simulator_bridge/config/default.json +ip=10.100.2.1 +port=5000 diff --git a/ros/src/system/autoware_health_checker/CMakeLists.txt b/ros/src/system/autoware_health_checker/CMakeLists.txt new file mode 100644 index 00000000000..e5ec6f64e46 --- /dev/null +++ b/ros/src/system/autoware_health_checker/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 2.8.3) +project(autoware_health_checker) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + autoware_system_msgs + roscpp + diagnostic_msgs + topic_tools + rostest + rosunit +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES node_status_publisher + CATKIN_DEPENDS autoware_system_msgs roscpp +# DEPENDS system_lib +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${autoware_system_msgs_INCLUDE_DIRS} +) + +set(NODE_STATUS_PUBLISHER_SRC + src/node_status_publisher.cpp + src/diag_buffer.cpp + src/rate_checker.cpp +) +add_library(node_status_publisher + ${NODE_STATUS_PUBLISHER_SRC} +) +target_link_libraries(node_status_publisher ${catkin_LIBRARIES}) +add_dependencies(node_status_publisher ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp) + +add_library(system_status_subscriber + src/system_status_subscriber.cpp +) +target_link_libraries(system_status_subscriber ${catkin_LIBRARIES}) +add_dependencies(system_status_subscriber ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp) + +add_executable(health_aggregator + src/health_aggregator_node.cpp + src/health_aggregator.cpp +) +target_link_libraries(health_aggregator ${catkin_LIBRARIES}) +add_dependencies(health_aggregator ${catkin_EXPORTED_TARGETS} autoware_system_msgs_generate_messages_cpp) + +# CPP Execution programs +set(CPP_EXEC_NAMES health_aggregator) +foreach(cpp_exec_names ${CPP_EXEC_NAMES}) + install(TARGETS ${cpp_exec_names} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +endforeach(cpp_exec_names) + +# include header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install library +install(TARGETS node_status_publisher system_status_subscriber + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test-autoware_health_checker + test/test_autoware_health_checker.test + test/src/test_autoware_health_checker.cpp + ${NODE_STATUS_PUBLISHER_SRC}) + target_link_libraries(test-autoware_health_checker + ${catkin_LIBRARIES}) +endif () \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h new file mode 100644 index 00000000000..0ddbe362d8c --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/constants.h @@ -0,0 +1,22 @@ +#ifndef CONSTANTS_H_INCLUDED +#define CONSTANTS_H_INCLUDED + +#include + +namespace autoware_health_checker +{ + constexpr uint8_t LEVEL_UNDEFINED = autoware_system_msgs::DiagnosticStatus::UNDEFINED; + constexpr uint8_t LEVEL_OK = autoware_system_msgs::DiagnosticStatus::OK; + constexpr uint8_t LEVEL_WARN = autoware_system_msgs::DiagnosticStatus::WARN; + constexpr uint8_t LEVEL_ERROR = autoware_system_msgs::DiagnosticStatus::ERROR; + constexpr uint8_t LEVEL_FATAL = autoware_system_msgs::DiagnosticStatus::FATAL; + + constexpr uint8_t TYPE_UNDEFINED = autoware_system_msgs::DiagnosticStatus::UNDEFINED; + constexpr uint8_t TYPE_OUT_OF_RANGE = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + constexpr uint8_t TYPE_RATE_IS_SLOW = autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW; + + constexpr double BUFFER_LENGTH = 5.0; + constexpr double UPDATE_RATE = 10.0; +} + +#endif //CONSTANTS_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/diag_buffer.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/diag_buffer.h new file mode 100644 index 00000000000..962739945b8 --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/diag_buffer.h @@ -0,0 +1,41 @@ +#ifndef DIAG_BUFFER_H_INCLUDED +#define DIAG_BUFFER_H_INCLUDED + +//headers in Autoare +#include +#include + +//headers in STL +#include +#include +#include +#include + +//headers in ROS +#include + +namespace autoware_health_checker +{ + class DiagBuffer + { + public: + DiagBuffer(std::string key,uint8_t type,std::string description,double buffer_length); + ~DiagBuffer(); + void addDiag(autoware_system_msgs::DiagnosticStatus status); + autoware_system_msgs::DiagnosticStatusArray getAndClearData(); + const uint8_t type; + const std::string description; + private: + std::mutex mtx_; + uint8_t getErrorLevel(); + void updateBuffer(); + std::string key_; + ros::Duration buffer_length_; + std::map buffer_; + autoware_system_msgs::DiagnosticStatusArray filterBuffer(ros::Time now, uint8_t level); + ros::Publisher status_pub_; + bool isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a, const autoware_system_msgs::DiagnosticStatus &b); + }; +} + +#endif //DIAG_BUFFER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h new file mode 100644 index 00000000000..089cc638822 --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/health_aggregator.h @@ -0,0 +1,44 @@ +#ifndef HEALTH_AGGREGATOR_H_INCLUDED +#define HEALTH_AGGREGATOR_H_INCLUDED + +//headers in ROS +#include +#include + +//headers in Autoware +#include +#include +#include + +//headers in boost +#include +#include +#include +#include +#include + +//headers in STL +#include +#include + +class HealthAggregator +{ +public: + HealthAggregator(ros::NodeHandle nh,ros::NodeHandle pnh); + ~HealthAggregator(); + void run(); +private: + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + ros::Publisher system_status_pub_; + ros::Subscriber node_status_sub_; + ros::Subscriber diagnostic_array_sub_; + void publishSystemStatus(); + void nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg); + void diagnosticArrayCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); + boost::optional convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg); + autoware_system_msgs::SystemStatus system_status_; + std::mutex mtx_; + void updateConnectionStatus(); +}; +#endif //HEALTH_AGGREGATOR_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h new file mode 100644 index 00000000000..7a4f30e78b2 --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h @@ -0,0 +1,87 @@ +#ifndef NODE_STATUS_PUBLISHER_H_INCLUDED +#define NODE_STATUS_PUBLISHER_H_INCLUDED + +//headers in ROS +#include + +//headers in Autoware +#include +#include +#include +#include + +//headers in STL +#include +#include +#include +#include + +//headers in boost +#include +#include +#include +#include +#include +#include + +namespace autoware_health_checker +{ + class NodeStatusPublisher + { + public: + NodeStatusPublisher(ros::NodeHandle nh,ros::NodeHandle pnh); + ~NodeStatusPublisher(); + void ENABLE(); + uint8_t CHECK_MIN_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value, std::string description); + uint8_t CHECK_MAX_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value, std::string description); + // std::pair first value is min value and second value is max value + uint8_t CHECK_RANGE(std::string key,double value,std::pair warn_value,std::pair error_value,std::pair fatal_value,std::string description); + template + uint8_t CHECK_VALUE(std::string key,T value,std::function check_func,std::function value_json_func,std::string description) + { + addNewBuffer(key,autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE,description); + uint8_t check_result = check_func(value); + boost::property_tree::ptree pt = value_json_func(value); + std::stringstream ss; + write_json(ss, pt); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + new_status.level = check_result; + new_status.description = description; + new_status.description = ss.str(); + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; + } + void CHECK_RATE(std::string key,double warn_rate,double error_rate,double fatal_rate,std::string description); + void NODE_ACTIVATE() + { + std::lock_guard lock(mtx_); + node_activated_ = true; + }; + void NODE_DEACTIVATE() + { + std::lock_guard lock(mtx_); + node_activated_ = false; + }; + bool getNodeStatus() + { + return node_activated_; + }; + private: + std::vector getKeys(); + std::vector getRateCheckerKeys(); + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + std::map > diag_buffers_; + std::map > rate_checkers_; + ros::Publisher status_pub_; + bool keyExist(std::string key); + void addNewBuffer(std::string key, uint8_t type, std::string description); + std::string doubeToJson(double value); + void publishStatus(); + bool node_activated_; + std::mutex mtx_; + }; +} +#endif //NODE_STATUS_PUBLISHER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h new file mode 100644 index 00000000000..c16447d99b5 --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/rate_checker.h @@ -0,0 +1,40 @@ +#ifndef RATE_CHECKER_H_INCLUDED +#define RATE_CHECKER_H_INCLUDED + +//headers in ROS +#include + +//headers in STL +#include +#include + +//headers in Boost +#include + +//headers in Autoware +#include + +namespace autoware_health_checker +{ + class RateChecker + { + public: + RateChecker(double buffer_length,double warn_rate,double error_rate,double fatal_rate,std::string description); + ~RateChecker(); + void check(); + std::pair getErrorLevelAndRate(); + uint8_t getErrorLevel(); + boost::optional getRate(); + const std::string description; + private: + ros::Time start_time_; + void update(); + std::vector data_; + const double buffer_length_; + const double warn_rate_; + const double error_rate_; + const double fatal_rate_; + std::mutex mtx_; + }; +} +#endif //RATE_CHECKER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber.h b/ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber.h new file mode 100644 index 00000000000..12fe8b3a4af --- /dev/null +++ b/ros/src/system/autoware_health_checker/include/autoware_health_checker/system_status_subscriber.h @@ -0,0 +1,34 @@ +#ifndef SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED +#define SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED + +//headers in Autoware +#include +#include + +//headers in ROS +#include + +//headers in STL +#include +#include + +namespace autoware_health_checker +{ + class SystemStatusSubscriber + { + public: + SystemStatusSubscriber(ros::NodeHandle nh,ros::NodeHandle pnh); + ~SystemStatusSubscriber(); + void enable(); + void addCallback(std::function func); + private: + void systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg); + std::mutex mtx_; + ros::Subscriber status_sub_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + std::vector > functions_; + }; +} + +#endif //SYSTEM_STATUS_SUBSCRIBER_H_INCLUDED \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/package.xml b/ros/src/system/autoware_health_checker/package.xml new file mode 100644 index 00000000000..21de50da288 --- /dev/null +++ b/ros/src/system/autoware_health_checker/package.xml @@ -0,0 +1,29 @@ + + + autoware_health_checker + 0.0.0 + The autoware_health_checker package + + MasayaKataoka + + Apache 2.0 + + catkin + autoware_system_msgs + roscpp + diagnostic_msgs + topic_tools + rostest + rosunit + autoware_system_msgs + roscpp + diagnostic_msgs + topic_tools + autoware_system_msgs + diagnostic_msgs + roscpp + topic_tools + + + + diff --git a/ros/src/system/autoware_health_checker/src/diag_buffer.cpp b/ros/src/system/autoware_health_checker/src/diag_buffer.cpp new file mode 100644 index 00000000000..040117342d6 --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/diag_buffer.cpp @@ -0,0 +1,94 @@ +#include + +namespace autoware_health_checker +{ + DiagBuffer::DiagBuffer(std::string key, uint8_t type, std::string description, double buffer_length) : type(type), description(description) + { + key_ = key; + buffer_length_ = ros::Duration(buffer_length); + } + + DiagBuffer::~DiagBuffer() + { + + } + + void DiagBuffer::addDiag(autoware_system_msgs::DiagnosticStatus status) + { + std::lock_guard lock(mtx_); + buffer_[status.level].status.emplace_back(status); + updateBuffer(); + return; + } + + autoware_system_msgs::DiagnosticStatusArray DiagBuffer::getAndClearData() + { + std::lock_guard lock(mtx_); + autoware_system_msgs::DiagnosticStatusArray data; + data = buffer_[autoware_health_checker::LEVEL_FATAL]; + data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_ERROR].status.begin(),buffer_[autoware_health_checker::LEVEL_ERROR].status.end()); + data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_WARN].status.begin(),buffer_[autoware_health_checker::LEVEL_WARN].status.end()); + data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_OK].status.begin(),buffer_[autoware_health_checker::LEVEL_OK].status.end()); + data.status.insert(data.status.end(),buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.begin(),buffer_[autoware_health_checker::LEVEL_UNDEFINED].status.end()); + std::sort(data.status.begin(), data.status.end(), std::bind(&DiagBuffer::isOlderTimestamp, this, std::placeholders::_1, std::placeholders::_2)); + buffer_.clear(); + return data; + } + + uint8_t DiagBuffer::getErrorLevel() + { + std::lock_guard lock(mtx_); + updateBuffer(); + if(buffer_[autoware_health_checker::LEVEL_FATAL].status.size() != 0) + { + return autoware_health_checker::LEVEL_FATAL; + } + else if(buffer_[autoware_health_checker::LEVEL_ERROR].status.size() != 0) + { + return autoware_health_checker::LEVEL_ERROR; + } + else if(buffer_[autoware_health_checker::LEVEL_WARN].status.size() != 0) + { + return autoware_health_checker::LEVEL_WARN; + } + else + { + return autoware_health_checker::LEVEL_OK; + } + } + + // filter data from timestamp and level + autoware_system_msgs::DiagnosticStatusArray DiagBuffer::filterBuffer(ros::Time now, uint8_t level) + { + autoware_system_msgs::DiagnosticStatusArray filterd_data; + autoware_system_msgs::DiagnosticStatusArray ret; + if(buffer_.count(level) != 0) + { + filterd_data = buffer_[level]; + } + for(auto data_itr = filterd_data.status.begin(); data_itr != filterd_data.status.end(); data_itr++) + { + if(data_itr->header.stamp> (now - buffer_length_)) + { + ret.status.push_back(*data_itr); + } + } + return ret; + } + + void DiagBuffer::updateBuffer() + { + ros::Time now = ros::Time::now(); + buffer_[autoware_health_checker::LEVEL_FATAL] = filterBuffer(now, autoware_health_checker::LEVEL_FATAL); + buffer_[autoware_health_checker::LEVEL_ERROR] = filterBuffer(now, autoware_health_checker::LEVEL_ERROR); + buffer_[autoware_health_checker::LEVEL_WARN] = filterBuffer(now, autoware_health_checker::LEVEL_WARN); + buffer_[autoware_health_checker::LEVEL_OK] = filterBuffer(now, autoware_health_checker::LEVEL_OK); + buffer_[autoware_health_checker::LEVEL_UNDEFINED] = filterBuffer(now, autoware_health_checker::LEVEL_UNDEFINED); + return; + } + + bool DiagBuffer::isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a, const autoware_system_msgs::DiagnosticStatus &b) + { + return a.header.stamp < b.header.stamp; + } +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/health_aggregator.cpp b/ros/src/system/autoware_health_checker/src/health_aggregator.cpp new file mode 100644 index 00000000000..b006279d85c --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/health_aggregator.cpp @@ -0,0 +1,114 @@ +#include + +HealthAggregator::HealthAggregator(ros::NodeHandle nh,ros::NodeHandle pnh) +{ + nh_ = nh; + pnh_ = pnh; +} + +HealthAggregator::~HealthAggregator() +{ + +} + +void HealthAggregator::run() +{ + system_status_pub_ = nh_.advertise("/system_status",10); + node_status_sub_ = nh_.subscribe("/node_status",10,&HealthAggregator::nodeStatusCallback,this); + diagnostic_array_sub_ = nh_.subscribe("/diagnostic_agg",10,&HealthAggregator::diagnosticArrayCallback,this); + boost::thread publish_thread(boost::bind(&HealthAggregator::publishSystemStatus, this)); + return; +} + +void HealthAggregator::publishSystemStatus() +{ + ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); + while(ros::ok()) + { + mtx_.lock(); + system_status_.header.stamp = ros::Time::now(); + updateConnectionStatus(); + system_status_pub_.publish(system_status_); + system_status_.node_status.clear(); + system_status_.hardware_status.clear(); + mtx_.unlock(); + rate.sleep(); + } + return; +} + +void HealthAggregator::updateConnectionStatus() +{ + std::vector detected_nodes; + ros::master::getNodes(detected_nodes); + system_status_.available_nodes = detected_nodes; + return; +} + +void HealthAggregator::nodeStatusCallback(const autoware_system_msgs::NodeStatus::ConstPtr msg) +{ + mtx_.lock(); + system_status_.node_status.push_back(*msg); + mtx_.unlock(); + return; +} + +void HealthAggregator::diagnosticArrayCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr msg) +{ + mtx_.lock(); + boost::optional status = convert(msg); + if(status) + { + system_status_.hardware_status.push_back(*status); + } + mtx_.unlock(); + return; +} + +boost::optional HealthAggregator::convert(const diagnostic_msgs::DiagnosticArray::ConstPtr msg) +{ + autoware_system_msgs::HardwareStatus status; + if(msg->status.size() == 0) + { + return boost::none; + } + status.header = msg->header; + for(auto diag_itr = msg->status.begin(); diag_itr != msg->status.end(); diag_itr++) + { + status.hardware_name = diag_itr->hardware_id; + autoware_system_msgs::DiagnosticStatus diag; + autoware_system_msgs::DiagnosticStatusArray diag_array; + diag.header = msg->header; + diag.key = diag_itr->hardware_id; + diag.description = diag_itr->message; + diag.type = autoware_system_msgs::DiagnosticStatus::HARDWARE; + if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::OK) + { + diag.level = autoware_health_checker::LEVEL_OK; + } + else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::WARN) + { + diag.level = autoware_health_checker::LEVEL_WARN; + } + else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::ERROR) + { + diag.level = autoware_health_checker::LEVEL_ERROR; + } + else if(diag_itr->level == diagnostic_msgs::DiagnosticStatus::STALE) + { + diag.level = autoware_health_checker::LEVEL_FATAL; + } + using namespace boost::property_tree; + std::stringstream ss; + ptree pt; + for(auto value_itr = diag_itr->values.begin(); value_itr != diag_itr->values.end(); value_itr++) + { + pt.put(value_itr->key+".string", value_itr->value); + } + write_json(ss, pt); + diag.value = ss.str(); + diag_array.status.push_back(diag); + status.status.push_back(diag_array); + } + return status; +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp b/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp new file mode 100644 index 00000000000..bea7288d2f0 --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/health_aggregator_node.cpp @@ -0,0 +1,14 @@ +#include + +#include + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "health_aggreator"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + HealthAggregator agg(nh,pnh); + agg.run(); + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp b/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp new file mode 100644 index 00000000000..d7a6521ab7c --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp @@ -0,0 +1,222 @@ +#include + +namespace autoware_health_checker +{ + NodeStatusPublisher::NodeStatusPublisher(ros::NodeHandle nh,ros::NodeHandle pnh) + { + node_activated_ = false; + nh_ = nh; + pnh_ = pnh; + status_pub_ = nh_.advertise("node_status",10); + } + + NodeStatusPublisher::~NodeStatusPublisher() + { + + } + + void NodeStatusPublisher::publishStatus() + { + ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE); + while(ros::ok()) + { + mtx_.lock(); + autoware_system_msgs::NodeStatus status; + status.node_activated = node_activated_; + ros::Time now = ros::Time::now(); + status.header.stamp = now; + status.node_name = ros::this_node::getName(); + std::vector checker_keys = getRateCheckerKeys(); + // iterate Rate checker and publish rate_check result + for(auto key_itr = checker_keys.begin(); key_itr != checker_keys.end(); key_itr++) + { + autoware_system_msgs::DiagnosticStatusArray diag_array; + autoware_system_msgs::DiagnosticStatus diag; + diag.type = autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW; + std::pair result = rate_checkers_[*key_itr]->getErrorLevelAndRate(); + diag.level = result.first; + diag.key = *key_itr; + diag.value = doubeToJson(result.second); + diag.description = rate_checkers_[*key_itr]->description; + diag.header.stamp = now; + diag_array.status.push_back(diag); + status.status.push_back(diag_array); + } + // iterate Diagnostic Buffer and publish all diagnostic data + std::vector keys = getKeys(); + for(auto key_itr = keys.begin(); key_itr != keys.end(); key_itr++) + { + status.status.push_back(diag_buffers_[*key_itr]->getAndClearData()); + } + status_pub_.publish(status); + mtx_.unlock(); + rate.sleep(); + } + return; + } + + void NodeStatusPublisher::ENABLE() + { + boost::thread publish_thread(boost::bind(&NodeStatusPublisher::publishStatus, this)); + return; + } + + std::vector NodeStatusPublisher::getKeys() + { + std::vector keys; + std::vector checker_keys = getRateCheckerKeys(); + std::pair > buf_itr; + BOOST_FOREACH(buf_itr,diag_buffers_) + { + bool matched = false; + for(auto checker_key_itr = checker_keys.begin(); checker_key_itr != checker_keys.end(); checker_key_itr++) + { + if(*checker_key_itr == buf_itr.first) + { + matched = true; + } + } + if(!matched) + { + keys.push_back(buf_itr.first); + } + } + return keys; + } + + std::vector NodeStatusPublisher::getRateCheckerKeys() + { + std::vector keys; + std::pair > checker_itr; + BOOST_FOREACH(checker_itr,rate_checkers_) + { + keys.push_back(checker_itr.first); + } + return keys; + } + + bool NodeStatusPublisher::keyExist(std::string key) + { + if(diag_buffers_.count(key) == 0) + { + return false; + } + return true; + } + + // add New Diagnostic Buffer if the key does not exist + void NodeStatusPublisher::addNewBuffer(std::string key, uint8_t type, std::string description) + { + if(!keyExist(key)) + { + std::shared_ptr buf_ptr = std::make_shared(key, type, description, autoware_health_checker::BUFFER_LENGTH); + diag_buffers_[key] = buf_ptr; + } + return; + } + + uint8_t NodeStatusPublisher::CHECK_MIN_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value,std::string description) + { + addNewBuffer(key,autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE,description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if(value < fatal_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } + else if(value < error_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } + else if(value < warn_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } + else + { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.description = description; + new_status.value = doubeToJson(value); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; + } + + uint8_t NodeStatusPublisher::CHECK_MAX_VALUE(std::string key,double value,double warn_value,double error_value,double fatal_value,std::string description) + { + addNewBuffer(key,autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE,description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if(value > fatal_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } + else if(value > error_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } + else if(value > warn_value) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } + else + { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.description = description; + new_status.value = doubeToJson(value); + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; + } + + uint8_t NodeStatusPublisher::CHECK_RANGE(std::string key,double value,std::pair warn_value,std::pair error_value,std::pair fatal_value,std::string description) + { + addNewBuffer(key,autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE,description); + autoware_system_msgs::DiagnosticStatus new_status; + new_status.type = autoware_system_msgs::DiagnosticStatus::OUT_OF_RANGE; + if(value < fatal_value.first || value > fatal_value.second) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::FATAL; + } + else if(value < error_value.first || value > error_value.second) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::ERROR; + } + else if(value < warn_value.first || value > warn_value.second) + { + new_status.level = autoware_system_msgs::DiagnosticStatus::WARN; + } + else + { + new_status.level = autoware_system_msgs::DiagnosticStatus::OK; + } + new_status.value = doubeToJson(value); + new_status.description = description; + new_status.header.stamp = ros::Time::now(); + diag_buffers_[key]->addDiag(new_status); + return new_status.level; + } + + void NodeStatusPublisher::CHECK_RATE(std::string key,double warn_rate,double error_rate,double fatal_rate,std::string description) + { + if(!keyExist(key)) + { + std::shared_ptr checker_ptr = std::make_shared(autoware_health_checker::BUFFER_LENGTH,warn_rate,error_rate,fatal_rate,description); + rate_checkers_[key] = checker_ptr; + } + addNewBuffer(key,autoware_system_msgs::DiagnosticStatus::RATE_IS_SLOW,description); + rate_checkers_[key]->check(); + return; + } + + std::string NodeStatusPublisher::doubeToJson(double value) + { + using namespace boost::property_tree; + std::stringstream ss; + ptree pt; + pt.put("value.double", value); + write_json(ss, pt); + return ss.str(); + } +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/rate_checker.cpp b/ros/src/system/autoware_health_checker/src/rate_checker.cpp new file mode 100644 index 00000000000..fdc4ed51bb2 --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/rate_checker.cpp @@ -0,0 +1,102 @@ +#include + +namespace autoware_health_checker +{ + RateChecker::RateChecker(double buffer_length,double warn_rate,double error_rate,double fatal_rate,std::string description) + : buffer_length_(buffer_length), warn_rate_(warn_rate), error_rate_(error_rate), fatal_rate_(fatal_rate), description(description) + { + start_time_ = ros::Time::now(); + } + + RateChecker::~RateChecker() + { + + } + + std::pair RateChecker::getErrorLevelAndRate() + { + std::pair ret; + boost::optional rate = getRate(); + if(!rate) + { + ret = std::make_pair(autoware_health_checker::LEVEL_ERROR,0); + } + else if(rate.get() < fatal_rate_) + { + ret = std::make_pair(autoware_health_checker::LEVEL_FATAL,rate.get()); + } + else if(rate.get() < error_rate_) + { + ret = std::make_pair(autoware_health_checker::LEVEL_ERROR,rate.get()); + } + else if(rate.get() < warn_rate_) + { + ret = std::make_pair(autoware_health_checker::LEVEL_WARN,rate.get()); + } + else + { + ret = std::make_pair(autoware_health_checker::LEVEL_OK,rate.get()); + } + return ret; + } + + uint8_t RateChecker::getErrorLevel() + { + boost::optional rate = getRate(); + if(!rate) + { + return autoware_health_checker::LEVEL_ERROR; + } + if(rate.get() < fatal_rate_) + { + return autoware_health_checker::LEVEL_FATAL; + } + if(rate.get() < error_rate_) + { + return autoware_health_checker::LEVEL_ERROR; + } + if(rate.get() < warn_rate_) + { + return autoware_health_checker::LEVEL_WARN; + } + return autoware_health_checker::LEVEL_OK; + } + + void RateChecker::check() + { + update(); + mtx_.lock(); + data_.push_back(ros::Time::now()); + mtx_.unlock(); + } + + void RateChecker::update() + { + mtx_.lock(); + std::vector buffer; + for(auto data_itr = data_.begin(); data_itr != data_.end(); data_itr++) + { + if(*data_itr > ros::Time::now()-ros::Duration(buffer_length_)) + { + buffer.push_back(*data_itr); + } + } + data_ = buffer; + mtx_.unlock(); + return; + } + + boost::optional RateChecker::getRate() + { + boost::optional rate; + if(ros::Time::now() - start_time_ < ros::Duration(buffer_length_)) + { + return boost::none; + } + update(); + mtx_.lock(); + rate = data_.size()/buffer_length_; + mtx_.unlock(); + return rate; + } +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp b/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp new file mode 100644 index 00000000000..692de2f3b24 --- /dev/null +++ b/ros/src/system/autoware_health_checker/src/system_status_subscriber.cpp @@ -0,0 +1,45 @@ +#include + +namespace autoware_health_checker +{ + SystemStatusSubscriber::SystemStatusSubscriber(ros::NodeHandle nh,ros::NodeHandle pnh) + { + nh_ = nh; + pnh_ = pnh; + } + + SystemStatusSubscriber::~SystemStatusSubscriber() + { + + } + + void SystemStatusSubscriber::enable() + { + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::Rate rate(1); + status_sub_ = nh_.subscribe("system_status",10,&SystemStatusSubscriber::systemStatusCallback,this); + while(ros::ok()) + { + rate.sleep(); + } + spinner.stop(); + return; + } + + void SystemStatusSubscriber::systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg) + { + for(auto function_itr = functions_.begin(); function_itr != functions_.end(); function_itr++) + { + std::function func = *function_itr; + func(*msg); + } + return; + } + + void SystemStatusSubscriber::addCallback(std::function func) + { + functions_.push_back(func); + return; + } +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp b/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp new file mode 100644 index 00000000000..352f188e02a --- /dev/null +++ b/ros/src/system/autoware_health_checker/test/src/test_autoware_health_checker.cpp @@ -0,0 +1,143 @@ +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * 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. + * + * + * v1.0 Masaya Kataoka + */ +#include +#include +#include + +class AutowareHealthCheckerTestSuite : public ::testing::Test +{ +public: + AutowareHealthCheckerTestSuite() + { + } + + ~AutowareHealthCheckerTestSuite() + { + } +}; + +class AutowareHealthCheckerTestClass +{ +public: + AutowareHealthCheckerTestClass() + { + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + node_status_publisher_ptr = std::make_shared(nh,pnh); + }; + std::shared_ptr node_status_publisher_ptr; +}; + +TEST(TestSuite, CHECK_MIN_VALUE) +{ + AutowareHealthCheckerTestClass test_autoware_health_checker; + uint8_t ret_fatal = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE("test",1,6,4,2,"test"); + ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) << "The value was self-diagnosed as fatal"; + uint8_t ret_error = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE("test",3,6,4,2,"test"); + ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) << "The value was self-diagnosed as error"; + uint8_t ret_warn = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE("test",5,6,4,2,"test"); + ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) << "The value was self-diagnosed as warn"; + uint8_t ret_ok = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MIN_VALUE("test",7,6,4,2,"test"); + ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) << "The value was self-diagnosed as ok"; +} + +TEST(TestSuite, CHECK_MAX_VALUE) +{ + AutowareHealthCheckerTestClass test_autoware_health_checker; + uint8_t ret_fatal = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE("test",7,2,4,6,"test"); + ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) << "The value was self-diagnosed as fatal"; + uint8_t ret_error = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE("test",5,2,4,6,"test"); + ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) << "The value was self-diagnosed as error"; + uint8_t ret_warn = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE("test",3,2,4,6,"test"); + ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) << "The value was self-diagnosed as warn"; + uint8_t ret_ok = test_autoware_health_checker.node_status_publisher_ptr->CHECK_MAX_VALUE("test",1,2,4,6,"test"); + ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) << "The value was self-diagnosed as ok"; +} + +TEST(TestSuite, CHECK_RANGE) +{ + AutowareHealthCheckerTestClass test_autoware_health_checker; + uint8_t ret_fatal = test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE("test",7.0,{2.0,4.0},{1.0,5.0},{0.0,6.0},"test"); + ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) << "The value was self-diagnosed as fatal"; + uint8_t ret_error = test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE("test",5.5,{2.0,4.0},{1.0,5.0},{0.0,6.0},"test"); + ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) << "The value was self-diagnosed as error"; + uint8_t ret_warn = test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE("test",4.5,{2.0,4.0},{1.0,5.0},{0.0,6.0},"test"); + ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) << "The value was self-diagnosed as warn"; + uint8_t ret_ok = test_autoware_health_checker.node_status_publisher_ptr->CHECK_RANGE("test",3.0,{2.0,4.0},{1.0,5.0},{0.0,6.0},"test"); + ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) << "The value was self-diagnosed as ok"; +} + +uint8_t test_function(double value) +{ +if(value == 0.0) +{ + return autoware_health_checker::LEVEL_FATAL; +} +if(value == 1.0) +{ + return autoware_health_checker::LEVEL_ERROR; +} +if(value == 2.0) +{ + return autoware_health_checker::LEVEL_WARN; +} +return autoware_health_checker::LEVEL_OK; +}; + +boost::property_tree::ptree test_value_json_func(double value) +{ +boost::property_tree::ptree tree; +tree.put("value", value); +return tree; +}; + +TEST(TestSuite, CHECK_VALUE) +{ + AutowareHealthCheckerTestClass test_autoware_health_checker; + std::function check_func = test_function; + std::function check_value_json_func = test_value_json_func; + uint8_t ret_fatal = test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE("test",0.0,check_func,check_value_json_func,"test"); + ASSERT_EQ(ret_fatal, autoware_health_checker::LEVEL_FATAL) << "The value was self-diagnosed as fatal"; + uint8_t ret_error = test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE("test",1.0,check_func,check_value_json_func,"test"); + ASSERT_EQ(ret_error, autoware_health_checker::LEVEL_ERROR) << "The value was self-diagnosed as fatal"; + uint8_t ret_warn = test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE("test",2.0,check_func,check_value_json_func,"test"); + ASSERT_EQ(ret_warn, autoware_health_checker::LEVEL_WARN) << "The value was self-diagnosed as fatal"; + uint8_t ret_ok = test_autoware_health_checker.node_status_publisher_ptr->CHECK_VALUE("test",-1.0,check_func,check_value_json_func,"test"); + ASSERT_EQ(ret_ok, autoware_health_checker::LEVEL_OK) << "The value was self-diagnosed as fatal"; + boost::optional value = check_value_json_func(0.0).get_optional("value"); + ASSERT_EQ(value.get(),0.0) << "The value must be true, failed to get json value"; +} + +TEST(TestSuite, NODE_STATUS) +{ + AutowareHealthCheckerTestClass test_autoware_health_checker; + test_autoware_health_checker.node_status_publisher_ptr->NODE_ACTIVATE(); + uint8_t ret_active = test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + ASSERT_EQ(ret_active, true) << "The value must be true"; + test_autoware_health_checker.node_status_publisher_ptr->NODE_DEACTIVATE(); + uint8_t ret_inactive = test_autoware_health_checker.node_status_publisher_ptr->getNodeStatus(); + ASSERT_EQ(ret_inactive, false) << "The value must be true"; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "AutowareHealthCheckerTestNode"); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test b/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test new file mode 100644 index 00000000000..d87a24411dd --- /dev/null +++ b/ros/src/system/autoware_health_checker/test/test_autoware_health_checker.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file