add autoware purepursut for test autoware pursuite.

src/ros/catkin/src/amathutils_lib/CHANGELOG.rst

@@ -0,0 +1,233 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package amathutils
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Rebuild decision maker (`#1609 <https://github.com/CPFL/Autoware/issues/1609>`_)
+* Fix license notice in corresponding package.xml
+* Contributors: amc-nu, s-azumi
+
+1.10.0 (2019-01-17)
+-------------------
+* Fixes for catkin_make
+* Use colcon as the build tool (`#1704 <https://github.com/CPFL/Autoware/issues/1704>`_)
+  * Switch to colcon as the build tool instead of catkin
+  * Added cmake-target
+  * Added note about the second colcon call
+  * Added warning about catkin* scripts being deprecated
+  * Fix COLCON_OPTS
+  * Added install targets
+  * Update Docker image tags
+  * Message packages fixes
+  * Fix missing dependency
+* Added missing function declaration to amathutils_lib.hpp
+* Contributors: Esteve Fernandez, Servando German, amc-nu
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+1.8.0 (2018-08-31)
+------------------
+* [Fix] Moved C++11 flag to autoware_build_flags (`#1395 <https://github.com/CPFL/Autoware/pull/1395>`_)
+* [Feature] Makes sure that all binaries have their dependencies linked (`#1385 <https://github.com/CPFL/Autoware/pull/1385>`_)
+* Contributors: Esteve Fernandez
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [Fix] rename packages (`#1269 <https://github.com/CPFL/Autoware/pull/1269>`_)
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * Rename road_wizard to trafficlight_recognizer
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * removed unnecessary comments
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename road_wizard to trafficlight_recognizer
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * Fix bug for not starting run time manager
+  * Remove invalid dependency
+* Contributors: Kosuke Murakami
+
+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
+* 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 support to stopline
+* Add feature of to find stopline. and following minor fixes
+  - to change vectormap operation to vectormap lib.
+  - to change state operation
+* add support to waypoint velocity control by state
+* add mps2kmph
+* update decisionmaker and related library
+  - add multiplelane path recognition
+  - renamed euc
+* Contributors: Yamato ANDO, Yusuke FUJII
+
+1.5.1 (2017-09-25)
+------------------
+
+1.5.0 (2017-09-21)
+------------------
+
+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)
+------------------

src/ros/catkin/src/amathutils_lib/CMakeLists.txt

@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(amathutils_lib) # autoware math utility
+
+find_package(autoware_build_flags REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+
+if (NOT EIGEN3_FOUND)
+    # Fallback to cmake_modules
+    find_package(cmake_modules REQUIRED)
+    find_package(Eigen REQUIRED)
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+    set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+    # Possibly map additional variables to the EIGEN3_ prefix.
+else ()
+    set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif ()
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_msgs
+  roscpp
+  roslint
+  tf
+  tf2
+)
+
+set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES amathutils_lib
+  CATKIN_DEPENDS
+    autoware_msgs
+    tf
+    tf2
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+add_library(amathutils_lib
+  src/Amathutils.cpp
+  src/kalman_filter.cpp
+  src/time_delay_kalman_filter.cpp
+  src/butterworth_filter.cpp
+)
+target_link_libraries(amathutils_lib
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(amathutils_lib
+  ${catkin_EXPORTED_TARGETS}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+install(TARGETS amathutils_lib
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+file(GLOB_RECURSE ROSLINT_FILES
+  LIST_DIRECTORIES false
+  *.cpp *.h *.hpp
+)
+
+list(APPEND ROSLINT_CPP_OPTS "--extensions=cc,h,hpp,cpp,cu,cuh" "--filter=-build/c++11,-runtime/references")
+roslint_cpp(${ROSLINT_FILES})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(amathutils-test test/test_amathutils_lib.test test/src/test_amathutils_lib.cpp)
+  target_link_libraries(amathutils-test ${catkin_LIBRARIES} amathutils_lib)
+  add_rostest_gtest(test-kalman_filter 
+    test/test_kalman_filter.test
+    test/src/test_kalman_filter.cpp
+    src/kalman_filter.cpp
+    src/time_delay_kalman_filter.cpp
+  )
+  target_link_libraries(test-kalman_filter ${catkin_LIBRARIES})
+
+  add_rostest_gtest(test-butterworth_filter 
+    test/test_butterworth_filter.test
+    test/src/test_butterworth_filter.cpp
+    src/butterworth_filter.cpp
+  )
+  target_link_libraries(test-butterworth_filter ${catkin_LIBRARIES})
+  roslint_add_test()
+endif()

src/ros/catkin/src/amathutils_lib/include/amathutils_lib/amathutils.hpp

@@ -0,0 +1,116 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#ifndef AMATHUTILS_LIB_AMATHUTILS_HPP
+#define AMATHUTILS_LIB_AMATHUTILS_HPP
+
+#include <cmath>
+#include <iostream>
+
+// ROS Messages
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <tf/tf.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace amathutils
+{
+
+#define G_MPSS 9.80665  // m/s^2
+
+inline double rad2deg(double _angle)
+{
+  return _angle * 180.0 / M_PI;
+}
+inline double deg2rad(double _angle)
+{
+  return _angle / 180.0 * M_PI;
+}
+
+inline double mps2kmph(double _mpsval)
+{
+  return (_mpsval * 3.6);  // mps * 60sec * 60minutes / 1000m
+}
+
+inline double kmph2mps(double _kmphval)
+{
+  return (_kmphval * 1000.0 / 60.0 / 60.0);  // kmph * 1000m / 60sec / 60sec
+}
+
+inline double getGravityAcceleration(double _acceleration_mpss)
+{
+  return _acceleration_mpss / G_MPSS;
+}
+
+inline double getAcceleration(double _v0, double _v, double _x)
+{
+  return (_v * _v - _v0 * _v0) / 2.0 / _x;
+}
+
+inline double getTimefromAcceleration(double _v0, double _v, double _a)
+{
+  return (_v - _v0) / _a;
+}
+
+bool getIntersect(double x1, double y1, double x2, double y2, double x3,
+  double y3, double x4, double y4, double* intersect_x, double* intersect_y);
+bool getIntersect(geometry_msgs::Point p1, geometry_msgs::Point p2, geometry_msgs::Point p3,
+  geometry_msgs::Point p4, geometry_msgs::Point* intersect);
+
+geometry_msgs::Point getNearPtOnLine(const geometry_msgs::Point &_p, const geometry_msgs::Point &_a,
+                                     const geometry_msgs::Point &_b);
+double find_distance(const geometry_msgs::Point &_from, const geometry_msgs::Point &_to);
+double find_distance(const geometry_msgs::Pose &_from, const geometry_msgs::Pose &_to);
+double find_angle(const geometry_msgs::Point &_from, const geometry_msgs::Point &_to);
+bool isIntersectLine(const geometry_msgs::Point &_l1_p1, const geometry_msgs::Point &_l1_p2,
+  const geometry_msgs::Point &_l2_p1, const geometry_msgs::Point &_l2_p2);
+int isPointLeftFromLine(const geometry_msgs::Point &_target, const geometry_msgs::Point &_line_p1,
+  const geometry_msgs::Point &_line_p2);
+
+/**
+ * @fn distanceFromSegment
+ * @brief calculates the distance between from a point to closest point on a line segment
+ * @param  _l1  first point of line segment        
+ * @param  _l2  second point of line segment        
+ * @param  _p   the point to find distance
+ * @return distance between point _p and line segment(_l1,_l2) 
+ */
+double distanceFromSegment(const geometry_msgs::Point &_l1, const geometry_msgs::Point &_l2,
+  const geometry_msgs::Point &_p);
+double getPoseYawAngle(const geometry_msgs::Pose &_pose);
+
+/**
+ * @brief convert from yaw to ros-Quaternion
+ * @param [in] yaw input yaw angle
+ * @return quaternion
+ */
+geometry_msgs::Quaternion getQuaternionFromYaw(const double &_yaw);
+
+/**
+ * @brief normalize angle into [-pi to pi]
+ * @param [in] _angle input angle
+ * @return normalized angle
+ */
+double normalizeRadian(const double _angle);
+
+double calcPosesAngleDiffRaw(const geometry_msgs::Pose &p_from, const geometry_msgs::Pose &_p_to);
+double calcPosesAngleDiffDeg(const geometry_msgs::Pose &_p_from, const geometry_msgs::Pose &_p_to);
+double calcPosesAngleDiffRad(const geometry_msgs::Pose &_p_from, const geometry_msgs::Pose &_p_to);
+
+}  // namespace amathutils
+
+#endif  // AMATHUTILS_LIB_AMATHUTILS_HPP

src/ros/catkin/src/amathutils_lib/include/amathutils_lib/butterworth_filter.hpp

@@ -0,0 +1,141 @@
+/*
+ * Copyright 2018-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.
+ *
+ * Authors: Ali Boyali, Simon Thompson
+ */
+
+#ifndef AMATHUTILS_LIB_BUTTERWORTH_FILTER_HPP
+#define AMATHUTILS_LIB_BUTTERWORTH_FILTER_HPP
+
+#include <cmath>
+#include <complex>
+#include <vector>
+
+struct Order_Cutoff
+{
+  int N;
+  double Wc;
+};
+
+struct DifferenceAnBn
+{
+  std::vector<double> An;
+  std::vector<double> Bn;
+};
+
+class ButterworthFilter
+{
+public:
+  // Prints the filter order and cutoff frequency
+
+  void PrintFilter_Specs();
+  void PrintFilter_ContinuousTimeRoots();
+
+  void PrintContinuousTimeTF();
+  void PrintDiscreteTimeTF();
+
+  void Buttord(double Wp, double Ws, double Ap, double As);
+
+  // resizes and intialises array for remembering previous filter values
+  void initializeForFiltering();
+
+  // Setters and Getters
+  void
+  setCuttoffFrequency(double Wc);  // Wc is the cut-off frequency in [rad/sec]
+
+  // fc is cut-off frequency in [Hz] and fs is the sampling frequency in [Hz]
+  void setCuttoffFrequency(double fc, double fs);
+  void setOrder(int N);
+
+  // Get the order, cut-off frequency and other filter properties
+  Order_Cutoff getOrderCutOff();
+  DifferenceAnBn getAnBn();
+
+  std::vector<double> getAn();
+  std::vector<double> getBn();
+
+  // computes continous time transfer function
+  void computeContinuousTimeTF(bool sampling_freqency = false);
+
+  // computes continous time transfer function
+  void computeDiscreteTimeTF(bool sampling_freqency = false);
+
+  double filter(const double &u);
+  void filtVector(const std::vector<double> &t, std::vector<double> &u,
+                  bool init_first_value = true);
+  void filtFiltVector(const std::vector<double> &t, std::vector<double> &u,
+                      bool init_first_value = true);
+
+private:
+  int mOrder = 0;                  // filter order
+  double mCutoff_Frequency = 0.0;  // filter cut-off frequency [rad/sec]
+
+  // Boolean parameter when a sampling frequency is defined. Default is false
+  bool prewarp = false;
+  double mSampling_Frequency = 1.0;
+
+  const double Td = 2.0;
+  // Gain of the discrete time function
+  std::complex<double> mDiscreteTimeGain{1.0, 0.0};
+
+  // Continuous time transfer function roots
+  std::vector<double> mPhaseAngles{0.0};
+  std::vector<std::complex<double>> mContinuousTimeRoots{{0.0, 0.0}};
+
+  // Discrete time zeros and roots
+  std::vector<std::complex<double>> mDiscreteTimeRoots{{0.0, 0.0}};
+  std::vector<std::complex<double>> mDiscreteTimeZeros{{-1.0, 0.0}};
+
+  // Continuous time transfer function numerator denominators
+  std::vector<std::complex<double>> mContinuousTimeDenominator{{0.0, 0.0}};
+  double mContinuousTimeNumerator = 0.0;
+
+  // Discrete time transfer function numerator denominators
+  std::vector<std::complex<double>> mDiscreteTimeDenominator{{0.0, 0.0}};
+  std::vector<std::complex<double>> mDiscreteTimeNumerator{{0.0, 0.0}};
+
+  // Numerator and Denominator Coefficients Bn and An of Discrete Time Filter
+
+  std::vector<double> mAn{0.0};
+  std::vector<double> mBn{0.0};
+
+  // METHODS
+  // polynomial function returns the coefficients given the roots of a
+  // polynomial
+  std::vector<std::complex<double>>
+  polynomialFromRoots(std::vector<std::complex<double>> &roots);
+
+  /*
+   * Implementation starts by computing the pole locations of the filter in the
+   * polar coordinate system . The algorithm first locates the poles  computing
+   * the phase angle and then poles as a complex number From the poles, the
+   * coefficients of denominator polynomial is calculated.
+   *
+   * Therefore, without phase, the roots cannot be calculated. The following
+   * three methods should be called successively.
+   *
+   * */
+
+  // computes the filter root locations in the polar coordinate system
+  void computePhaseAngles();
+
+  // Computes continuous time roots from the phase angles
+  void computeContinuousTimeRoots(bool use_sampling_freqency = false);
+
+  std::vector<double> u_unfiltered;
+  std::vector<double> u_filtered;
+};
+
+#endif  // AMATHUTILS_LIB_BUTTERWORTH_FILTER_HPP

src/ros/catkin/src/amathutils_lib/include/amathutils_lib/kalman_filter.hpp

@@ -0,0 +1,203 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#ifndef AMATHUTILS_LIB_KALMAN_FILTER_HPP
+#define AMATHUTILS_LIB_KALMAN_FILTER_HPP
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+
+/**
+ * @file kalman_filter.h
+ * @brief kalman filter class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  KalmanFilter();
+
+  /**
+   * @brief constructor with initialization
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariace matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  KalmanFilter(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+               const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+               const Eigen::MatrixXd &P);
+
+  /**
+   * @brief destructor
+   */
+  ~KalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariace matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  bool init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+            const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+            const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+            const Eigen::MatrixXd &P);
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P initial covariance of estimated state
+   */
+  bool init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0);
+
+  /**
+   * @brief set A of process model
+   * @param A coefficient matrix of x for process model
+   */
+  void setA(const Eigen::MatrixXd &A);
+
+  /**
+   * @brief set B of process model
+   * @param B coefficient matrix of u for process model
+   */
+  void setB(const Eigen::MatrixXd &B);
+
+  /**
+   * @brief set C of measurement model
+   * @param C coefficient matrix of x for measurement model
+   */
+  void setC(const Eigen::MatrixXd &C);
+
+  /**
+   * @brief set covariace matrix Q for process model
+   * @param Q covariace matrix for process model
+   */
+  void setQ(const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief set covariance matrix R for measurement model
+   * @param R covariance matrix for measurement model
+   */
+  void setR(const Eigen::MatrixXd &R);
+
+  /**
+   * @brief get current kalman filter state
+   * @param x kalman filter state
+   */
+  void getX(Eigen::MatrixXd &x);
+
+  /**
+   * @brief get current kalman filter covariance
+   * @param P kalman filter covariance
+   */
+  void getP(Eigen::MatrixXd &P);
+
+  /**
+   * @brief get component of current kalman filter state
+   * @param i index of kalman filter state
+   * @return value of i's component of the kalman filter state x[i]
+   */
+  double getXelement(unsigned int i);
+
+  /**
+   * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. This is mainly for EKF with variable matrix.
+   * @param u input for model
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param Q covariace matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &u, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @param Q covariace matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+               const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A);
+
+  /**
+   * @brief calculate kalman filter state by prediction model with A, B and Q being class menber variables.
+   * @param u input for the model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd &u);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is mainly for EKF with variable matrix.
+   * @param y measured values
+   * @param y output values expected from measurement model
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &y_pred,
+              const Eigen::MatrixXd &C, const Eigen::MatrixXd &R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly for EKF with variable matrix.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+              const Eigen::MatrixXd &R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R being class menber variables.
+   * @param y measured values
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd &y);
+
+protected:
+  Eigen::MatrixXd x_;  //!< @brief current estimated state
+  Eigen::MatrixXd A_;  //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd B_;  //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd C_;  //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd Q_;  //!< @brief covariace matrix for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd P_;  //!< @brief covariance of estimated state
+};
+
+#endif  // AMATHUTILS_LIB_KALMAN_FILTER_HPP

src/ros/catkin/src/amathutils_lib/include/amathutils_lib/time_delay_kalman_filter.hpp

@@ -0,0 +1,85 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#ifndef AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP
+#define AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP
+
+#include <iostream>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include "amathutils_lib/kalman_filter.hpp"
+
+/**
+ * @file time_delay_kalman_filter.h
+ * @brief kalman filter with delayed measurement class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class TimeDelayKalmanFilter : public KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  TimeDelayKalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P0 initial covariance of estimated state
+   * @param max_delay_step Maximum number of delay steps, which determines the dimension of the extended kalman filter
+   */
+  void init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P, const int max_delay_step);
+
+  /**
+   * @brief get latest time estimated state
+   * @param x latest time estimated state
+   */
+  void getLatestX(Eigen::MatrixXd &x);
+
+  /**
+   * @brief get latest time estimation covariance
+   * @param P latest time estimation covariance
+   */
+  void getLatestP(Eigen::MatrixXd &P);
+
+  /**
+   * @brief calculate kalman filter covariance by predicion model with time delay. This is mainly for EKF of nonlinear process model.
+   * @param x_next predicted state by prediction model
+   * @param A coefficient matrix of x for process model
+   * @param Q covariace matrix for process model
+   */
+  bool predictWithDelay(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+                        const Eigen::MatrixXd &Q);
+
+  /**
+   * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly for EKF of nonlinear process model.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @param delay_step measurement delay
+   */
+  bool updateWithDelay(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                       const Eigen::MatrixXd &R, const int delay_step);
+
+private:
+  int max_delay_step_;  //!< @brief maximum number of delay steps
+  int dim_x_;           //!< @brief dimension of latest state
+  int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
+};
+
+#endif  // AMATHUTILS_LIB_TIME_DELAY_KALMAN_FILTER_HPP

src/ros/catkin/src/amathutils_lib/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>amathutils_lib</name>
+  <version>1.12.0</version>
+  <description>The libamathutils package</description>
+  <maintainer email="yusuke.fujii@tier4.jp">Yusuke FUJII</maintainer>
+  <author email="yusuke.fujii@tier4.jp">Yusuke FUJII</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <test_depend>rostest</test_depend>
+
+  <depend>autoware_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>roslint</depend>
+  <depend>tf2</depend>
+  <depend>tf</depend>
+  <depend>eigen</depend>
+</package>

src/ros/catkin/src/amathutils_lib/src/Amathutils.cpp

@@ -0,0 +1,218 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#include <amathutils_lib/amathutils.hpp>
+
+namespace amathutils
+{
+geometry_msgs::Point getNearPtOnLine(const geometry_msgs::Point &_p, const geometry_msgs::Point &_a,
+                                     const geometry_msgs::Point &_b)
+{
+  double len = find_distance(_a, _b);
+
+  geometry_msgs::Point vnab;
+  geometry_msgs::Point vap;
+  geometry_msgs::Point ret;
+
+  vnab.x = (_b.x - _a.x) / len;
+  vnab.y = (_b.y - _a.y) / len;
+  vnab.z = (_b.z - _a.z) / len;
+
+  vap.x = _p.x - _a.x;
+  vap.y = _p.y - _a.y;
+  vap.z = _p.z - _a.z;
+
+  double dist_ax = vnab.x * vap.x + vnab.y * vap.y + vnab.z * vap.z;
+
+  ret.x = _a.x + (vnab.x * dist_ax);
+  ret.y = _a.y + (vnab.y * dist_ax);
+  ret.z = _a.z + (vnab.z * dist_ax);
+
+  return ret;
+}
+double find_distance(const geometry_msgs::Point &_from, const geometry_msgs::Point &_to)
+{
+  return std::hypot(std::hypot(_from.x - _to.x, _from.y - _to.y), _from.z - _to.z);
+}
+double find_distance(const geometry_msgs::Pose &_from, const geometry_msgs::Pose &_to)
+{
+  return find_distance(_from.position, _to.position);
+}
+
+double find_angle(const geometry_msgs::Point &_from, const geometry_msgs::Point &_to)
+{
+  double _angle = std::atan2(_to.y - _from.y, _to.x - _from.x);
+  if (_angle < 0.0)
+    _angle = _angle + 2 * M_PI;
+
+  return _angle * 360 / (2 * M_PI);
+}
+
+bool isIntersectLine(const geometry_msgs::Point &_l1_p1, const geometry_msgs::Point &_l1_p2,
+                     const geometry_msgs::Point &_l2_p1, const geometry_msgs::Point &_l2_p2)
+{
+  const double ta = (_l2_p1.x - _l2_p2.x) * (_l1_p1.y - _l2_p1.y) + (_l2_p1.y - _l2_p2.y) * (_l2_p1.x - _l1_p1.x);
+  const double tb = (_l2_p1.x - _l2_p2.x) * (_l1_p2.y - _l2_p1.y) + (_l2_p1.y - _l2_p2.y) * (_l2_p1.x - _l1_p2.x);
+  const double tc = (_l1_p1.x - _l1_p2.x) * (_l2_p1.y - _l1_p1.y) + (_l1_p1.y - _l1_p2.y) * (_l1_p1.x - _l2_p1.x);
+  const double td = (_l1_p1.x - _l1_p2.x) * (_l2_p2.y - _l1_p1.y) + (_l1_p1.y - _l1_p2.y) * (_l1_p1.x - _l2_p2.x);
+
+  if (tc * td < 0 && ta * tb < 0)
+    return true;
+  else
+    return false;
+}
+
+/*
+ *        |
+ *     .　|       =>LEFT = 1
+ *     　 |   .   =>RIGHT = -1
+ *      　|　
+ */
+
+int isPointLeftFromLine(const geometry_msgs::Point &_target, const geometry_msgs::Point &_line_p1,
+                        const geometry_msgs::Point &_line_p2)
+{
+  const int LEFT = 1;
+  const int RIGHT = -1;
+  const int ONLINE = 0;
+  const double n = _target.x * (_line_p1.y - _line_p2.y) + _line_p1.x * (_line_p2.y - _target.y) +
+                   _line_p2.x * (_target.y - _line_p1.y);
+
+  return n > 0 ? LEFT : n < 0 ? RIGHT : ONLINE;
+}
+
+
+// Following implementation comes from the website below:
+// Author: Dan Sunday
+// site: http://geomalgorithms.com/a02-_lines.html
+// viewed: 2019/5/20
+double distanceFromSegment(
+  const geometry_msgs::Point &_l1, const geometry_msgs::Point &_l2, const geometry_msgs::Point &_p)
+{
+  geometry_msgs::Point v, w;
+  v.x = _l2.x - _l1.x;
+  v.y = _l2.y - _l1.y;
+  v.z = _l2.z - _l1.z;
+
+  w.x = _p.x - _l1.x;
+  w.y = _p.y - _l1.y;
+  w.z = _p.z - _l1.z;
+
+  double dot_vw = v.x * w.x + v.y * w.y + v.z * w.z;
+  double dot_vv = v.x * v.x + v.y * v.y + v.z * v.z;
+
+  if (dot_vw <= 0)
+  {
+    return find_distance(_p, _l1);
+  }
+
+  if (dot_vv <= dot_vw)
+  {
+    return find_distance(_p, _l2);
+  }
+
+  double b = dot_vw / dot_vv;
+  geometry_msgs::Point pb;
+  pb.x = _l1.x + b * v.x;
+  pb.y = _l1.y + b * v.y;
+  pb.z = _l1.z + b * v.z;
+
+  return find_distance(_p, pb);
+}
+
+double getPoseYawAngle(const geometry_msgs::Pose &_pose)
+{
+  double r, p, y;
+
+  // get Quaternion
+  tf::Quaternion quat(_pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);
+  // converted to RPY[-pi : pi]
+  tf::Matrix3x3(quat).getRPY(r, p, y);
+  return y;
+}
+
+geometry_msgs::Quaternion getQuaternionFromYaw(const double &_yaw)
+{
+  tf2::Quaternion q;
+  q.setRPY(0, 0, _yaw);
+  return tf2::toMsg(q);
+}
+
+double calcPosesAngleDiffRaw(const geometry_msgs::Pose &p_from, const geometry_msgs::Pose &_p_to)
+{
+  return getPoseYawAngle(p_from) - getPoseYawAngle(_p_to);
+}
+
+double normalizeRadian(const double _angle)
+{
+  double n_angle = std::fmod(_angle, 2 * M_PI);
+  n_angle = n_angle > M_PI ? n_angle - 2 * M_PI : n_angle < -M_PI ? 2 * M_PI + n_angle : n_angle;
+
+  // another way
+  // Math.atan2(Math.sin(_angle), Math.cos(_angle));
+  return n_angle;
+}
+
+double calcPosesAngleDiffDeg(const geometry_msgs::Pose &_p_from, const geometry_msgs::Pose &_p_to)
+{
+  // convert to [-pi : pi]
+  return rad2deg(normalizeRadian(calcPosesAngleDiffRaw(_p_from, _p_to)));
+}
+
+double calcPosesAngleDiffRad(const geometry_msgs::Pose &_p_from, const geometry_msgs::Pose &_p_to)
+{
+  // convert to [-pi : pi]
+  return normalizeRadian(calcPosesAngleDiffRaw(_p_from, _p_to));
+}
+
+bool getIntersect(geometry_msgs::Point p1, geometry_msgs::Point p2, geometry_msgs::Point p3,
+  geometry_msgs::Point p4, geometry_msgs::Point* intersect)
+{
+  return getIntersect(p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p4.x, p4.y, &intersect->x, &intersect->y);
+}
+
+bool getIntersect(double x1, double y1, double x2, double y2, double x3, double y3, double x4,
+  double y4, double* intersect_x, double* intersect_y )
+{
+    // let p1(x1, y1), p2(x2, y2), p3(x3, y3), p4(x4,y4)
+    // intersect of line segment p1 to p2 and p3 to p4 satisfies
+    // p1 + r(p2 - p1) = p3 + s(p4 - p3)
+    // 0 <= r <= 1
+    // 0 <= s <= 1
+    double denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+
+    if (denominator == 0)
+    {
+      // line is parallel
+      return false;
+    }
+
+    double r = ( (y4 - y3) * (x3 - x1) - (x4 - x3) * (y3 - y1) ) / denominator;
+    double s = ( (y2 - y1) * (x3 - x1) - (x2 - x1) * (y3 - y1) ) / denominator;
+
+    if (r >= 0 && r <= 1 && s >= 0 && s <= 1)
+    {
+      *intersect_x = x1 + r * (x2 - x1);
+      *intersect_y = y1 + r * (y2 - y1);
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+}
+
+}  // namespace amathutils

src/ros/catkin/src/amathutils_lib/src/README.md

@@ -0,0 +1,156 @@
+### Butterworth Low-pass Filter Design Tool Class 
+
+This Butterworth low-pass filter design tool can be used to design a Butterworth filter in continuous and discrete-time from the given specifications of the filter performance. The Butterworth filter is a class implementation. The object is created by a default constructor without any argument. 
+
+The filter can be prepared in three ways. If the filter specifications are known such as the passband, stopband
+frequencies (Wp and Ws) together with the passband and stopband ripple magnitudes (Ap and As), one can call the
+filter's buttord method with these arguments to obtain the recommended filter order (N) and cut-off frequency 
+(Wc  [rad/s]). An example call is demonstrated below;
+
+    ButterworthFilter bf();
+
+    Wp = 2.0; // passband frequency [rad/sec]
+    Ws = 3.0; // stopband frequency [rad/sec]
+    Ap = 6.0; // passband ripple mag or loss [dB]
+    As = 20.0; // stop band rippe attenuation [dB]
+    
+    // Computing filter coefficients from the specs
+    bf.Buttord(Wp, Ws, Ap, As);
+
+    // Get the computed order and Cut-off frequency
+    Order_Cutoff NWc = bf.getOrderCutOff();]
+    
+    cout << " The computed order is ;" << NWc.N << endl;
+    cout << " The computed cut-off frequency is ;" << NWc.Wc << endl;
+    
+The filter order and cut-off frequency can be obtained in a struct using bf.getOrderCutoff() method. These specs can be
+printed on the screen by calling PrintFilterSpecs() method. If the user would like to define the order and cut-off
+frequency manually, the setter methods for these variables can be called to set the filter order (N) and the desired
+cut-off frequency (Wc [rad/sec]) for the filter.
+
+#### Obtaining Filter Transfer Functions
+The discrete transfer function of the filter requires the roots and gain of the continuous-time transfer function. 
+Therefore, it is a must to call the first computeContinuousTimeTF() to create the continuous-time transfer of the method; 
+    
+    bf.computeContinuousTimeTF();
+    
+The computed continuous-time transfer function roots can be printed on the screen using the methods;  
+
+    bf.PrintFilter_ContinuousTimeRoots();
+    bf.PrintContinuousTimeTF();
+    
+The resulting screen output for a 5th order filter is demonstrated below. 
+
+     Roots of Continous Time Filter Transfer Function Denominator are : 
+    -0.585518 + j 1.80204
+    -1.53291 + j 1.11372
+    -1.89478 + j 2.32043e-16
+    -1.53291 + j -1.11372
+    -0.585518 + j -1.80204
+    
+    
+    The Continuous-Time Transfer Function of the Filter is ;
+    
+             24.422 
+    --------------------------------------------------------
+    1.000 *s[5] + 6.132 *s[4] + 18.798 *s[3] + 35.619 *s[2] + 41.711 *s[1] + 24.422 
+    
+
+#### Discrete Time Transfer Function (Difference Equations)
+
+The digital filter equivalent of the continuous-time definitions is produced by using the bilinear transformation. 
+When creating the discrete-time function of the ButterworthFilter object, its Numerator (Bn) and Denominator (An
+) coefficients are stored in a vector of filter order size N.
+
+The discrete transfer function method is called using ;
+
+    bf.computeDiscreteTimeTF();
+    bf.PrintDiscreteTimeTF();
+    
+The results are printed on the screen like;
+    The Discrete-Time Transfer Function of the Filter is ;
+
+    0.191 *z[-5] + 0.956 *z[-4] + 1.913 *z[-3] + 1.913 *z[-2] + 0.956 *z[-1] + 0.191
+    --------------------------------------------------------
+    1.000 *z[-5] + 1.885 *z[-4] + 1.888 *z[-3] + 1.014 *z[-2] + 0.298 *z[-1] + 0.037
+    
+and the associated difference coefficients An and Bn by withing a struct ; 
+
+    DifferenceAnBn AnBn = bf.getAnBn();
+    
+ The difference coefficients appear in the filtering equation in the form of.  
+ 
+    An * Y_filtered = Bn * Y_unfiltered  
+ 
+To filter a signal given in a vector form ;
+
+#### Calling Filter by a specified cut-off and sampling frequencies [in Hz] 
+
+The Butterworth filter can also be created by defining the desired order (N), a cut-off frequency (fc in [Hz]) and a
+ sampling frequency (fs in [Hz]). In this method, the cut-off frequency is pre warped with respect to the sampling frequency [1, 2] to match the continuous and digital filter frequencies. 
+ 
+ The filter is prepared by the following calling options;
+ 
+     // 3rd METHOD defining a sampling frequency together with the cut-off fc, fs
+     bf.setOrder(2);
+     bf.setCuttoffFrequency(10, 100);
+
+At this step, we define a boolean variable whether to use the pre-warping option or not. 
+
+    // Compute Continuous Time TF
+    bool use_sampling_frequency = true;
+    bf.computeContinuousTimeTF(use_sampling_frequency);
+    bf.PrintFilter_ContinuousTimeRoots();
+    bf.PrintContinuousTimeTF();
+
+    // Compute Discrete Time TF
+    bf.computeDiscreteTimeTF(use_sampling_frequency);
+    bf.PrintDiscreteTimeTF();
+ 
+
+#### Filtering Data
+
+Before starting filtering, an intialization function should be called to set up data structures to store the filter history:
+
+     bf.initializeForFiltering();
+
+To filter data using the Butterworth filter 3 methodsfor filtering data have been implemented:
+
+1) Apply filter to a single discrete data point
+
+        // unfiltered data point
+        double u = ....;
+        // filtered data
+        double u_f;
+     
+        u_f = bf.filter(u)
+
+2) Apply filter to a vector of discrete time series data
+
+        // unfiltered_data
+        std::vector<double> u;
+        // filtered_data
+        std::vector<double> u_f;
+
+        bf.filt_vector(u, u_f);
+
+3) Apply filter twice (forward and reverse) to a vector of discrete time series data
+
+        // unfiltered_data
+        std::vector<double> u;
+        // filtered_data
+        std::vector<double> u_f;
+
+        bf.filtfilt_vector(u, u_f);
+   
+
+
+ References:
+  
+ 1. Manolakis, Dimitris G., and Vinay K. Ingle. Applied digital signal processing: theory and practice.  Cambridge
+  University Press, 2011. 
+  
+ 2. https://en.wikibooks.org/wiki/Digital_Signal_Processing/Bilinear_Transform
+ 
+ 
+    

src/ros/catkin/src/amathutils_lib/src/butterworth_filter.cpp

@@ -0,0 +1,450 @@
+/*
+ * Copyright 2018-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.
+ *
+ * Aauthor Ali Boyali, Simon Thompson
+ */
+
+#include <algorithm>
+#include <cmath>
+#include <complex>
+#include <cstdio>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "amathutils_lib/butterworth_filter.hpp"
+
+void ButterworthFilter::Buttord(double Wp, double Ws, double Ap, double As)
+{
+  /*
+   *  Inputs are double Wp, Ws, Ap, As;
+   *  Wp; passband frequency [rad/sc],
+   *  Ws: stopband frequeny [rad/sc],
+   *  Ap. {As}: pass {stop} band ripple [dB]
+   *
+   * */
+
+  double alpha, beta;
+
+  alpha = Ws / Wp;
+  beta = sqrt((pow(10, As / 10.0) - 1.0) / (pow(10, Ap / 10.0) - 1.0));
+  int order = std::ceil(log(beta) / log(alpha));
+
+  setOrder(order);
+
+  // right limit, left limit
+  /*
+   * The left and right limits of the magnitutes satisfy the specs at the
+   * frequencies Ws and Wp Scipy.buttord gives left limit as the cut-off
+   * frequency whereas Matlab gives right limit
+   *
+   * */
+  double right_lim =
+      Ws * (pow((pow(10.0, As / 10.0) - 1.0), -1.0 / (2 * mOrder)));
+  //    double left_lim = Wp * (pow((pow(10.0, Ap / 10.0) - 1.0), -1.0 / (2 *
+  //    mOrder)));
+
+  // mCutoff_Frequency = left_lim;
+  setCuttoffFrequency(right_lim);
+}
+
+void ButterworthFilter::initializeForFiltering()
+{
+  // record history of filtered and unfiltered signal for applying filter
+  u_unfiltered.resize(mOrder + 1, 0.0);
+  u_filtered.resize(mOrder + 1, 0.0);
+}
+void ButterworthFilter::setOrder(int N)
+{
+  mOrder = N;
+  initializeForFiltering();
+}
+
+void ButterworthFilter::setCuttoffFrequency(double Wc)
+{
+  mCutoff_Frequency = Wc;
+}
+
+void ButterworthFilter::setCuttoffFrequency(double fc, double fs)
+{
+  /*
+   * fc is the cut-off frequency in [Hz]
+   * fs is the sampling frequency in [Hz]
+   * */
+  if (fc >= fs / 2)
+  {
+    std::cout << "\n  ERROR: Cut-off frequency  fc must be less than fs/2 \n";
+    _Exit(0);
+  }
+  mCutoff_Frequency = fc * 2.0 * M_PI;
+  mSampling_Frequency = fs;
+}
+
+std::vector<std::complex<double>>
+ButterworthFilter::polynomialFromRoots(std::vector<std::complex<double>> &rts)
+{
+  std::vector<std::complex<double>> roots = rts;
+  std::vector<std::complex<double>> coefficients(roots.size() + 1, {0, 0});  // NOLINT
+
+  int n = roots.size();
+
+  coefficients[0] = {1.0, 0.0};
+
+  // Use Vieta's Formulas to calculate polynomial coefficients
+  // relates sum and products of roots to coefficients
+  for (int i = 0; i < n; i++)
+  {
+    for (int j = i; j != -1; j--)
+    {
+      coefficients[j + 1] = coefficients[j + 1] - (roots[i] * coefficients[j]);
+    }
+  }
+
+  return coefficients;
+}
+
+void ButterworthFilter::computePhaseAngles()
+{
+  mPhaseAngles.resize(mOrder);
+  int i = 1;
+
+  for (auto &&x : mPhaseAngles)
+  {
+    x = M_PI_2 + (M_PI * (2.0 * i - 1.0) / (2.0 * mOrder));
+    i++;
+  }
+}
+
+void ButterworthFilter::computeContinuousTimeRoots(bool use_sampling_freqency)
+{
+  // First compute  the phase angles of the roots
+  computePhaseAngles();
+
+  mContinuousTimeRoots.resize(mOrder, {0.0, 0.0});  // NOLINT
+  int i = 0;
+
+  if (use_sampling_freqency)
+  {
+    double Fc = (mSampling_Frequency / M_PI) *
+                tan(mCutoff_Frequency / (mSampling_Frequency * 2.0));
+
+    for (auto &&x : mContinuousTimeRoots)
+    {
+      x = {cos(mPhaseAngles[i]) * Fc * 2.0 * M_PI,   // NOLINT
+           sin(mPhaseAngles[i]) * Fc * 2.0 * M_PI};  // NOLINT
+      i++;
+    }
+  }
+  else
+  {
+    for (auto &&x : mContinuousTimeRoots)
+    {
+      x = {mCutoff_Frequency * cos(mPhaseAngles[i]),   // NOLINT
+           mCutoff_Frequency * sin(mPhaseAngles[i])};  // NOLINT
+      i++;
+    }
+  }
+}
+
+void ButterworthFilter::computeContinuousTimeTF(bool use_sampling_freqency)
+{
+  computeContinuousTimeRoots(use_sampling_freqency);
+  mContinuousTimeDenominator.resize(mOrder + 1, {0.0, 0.0});  // NOLINT
+
+  mContinuousTimeDenominator = polynomialFromRoots(mContinuousTimeRoots);
+  mContinuousTimeNumerator = pow(mCutoff_Frequency, mOrder);
+}
+
+void ButterworthFilter::computeDiscreteTimeTF(bool use_sampling_freqency)
+{
+  /* @brief
+   * This method assumes the continous time transfer function of filter has
+   * already been computed and stored in the
+   * object.
+   *
+   * */
+  // Resizes the roots and zeros to the new order of discrete time then fills
+  // the values by Bilinear Transformation
+
+  mDiscreteTimeZeros.resize(
+      mOrder, {-1.0, 0.0}); // Butter puts zeros at -1.0 for causality  // NOLINT
+  mDiscreteTimeRoots.resize(mOrder, {0.0, 0.0});  // NOLINT
+  mAn.resize(mOrder + 1, 0.0);
+  mBn.resize(mOrder + 1, 0.0);
+
+  mDiscreteTimeGain = {mContinuousTimeNumerator, 0.0};
+
+  // Bi-linear Transformation of the Roots
+  int i = 0;
+
+  if (use_sampling_freqency)
+  {
+    for (auto &&dr : mDiscreteTimeRoots)
+    {
+      dr = (1.0 + mContinuousTimeRoots[i] / (mSampling_Frequency * 2.0)) /
+           (1.0 - mContinuousTimeRoots[i] / (mSampling_Frequency * 2.0));
+      i++;
+    }
+
+    mDiscreteTimeDenominator = polynomialFromRoots(mDiscreteTimeRoots);
+
+    // Obtain the coefficients of numerator and denominator
+    i = 0;
+    mDiscreteTimeNumerator = polynomialFromRoots(mDiscreteTimeZeros);
+
+    // Compute Discrete Time Gain
+    std::complex<double> sum_num{0.0, 0.0};
+    std::complex<double> sum_den{0.0, 0.0};
+
+    for (auto &n : mDiscreteTimeNumerator)
+    {
+      sum_num += n;
+    }
+
+    for (auto &n : mDiscreteTimeDenominator)
+    {
+      sum_den += n;
+    }
+
+    mDiscreteTimeGain = (sum_den / sum_num);
+
+    for (auto &&dn : mDiscreteTimeNumerator)
+    {
+      dn = dn * mDiscreteTimeGain;
+      mBn[i] = dn.real();
+      i++;
+    }
+
+    i = 0;
+    for (auto &&dd : mDiscreteTimeDenominator)
+    {
+      mAn[i] = dd.real();
+      i++;
+    }
+  }
+  else
+  {
+    for (auto &&dr : mDiscreteTimeRoots)
+    {
+      dr = (1.0 + 2.0 * mContinuousTimeRoots[i] / 2.0) /
+           (1.0 - Td * mContinuousTimeRoots[i] / 2.0);
+
+      mDiscreteTimeGain = mDiscreteTimeGain / (1.0 - mContinuousTimeRoots[i]);
+      i++;
+    }
+
+    mDiscreteTimeDenominator = polynomialFromRoots(mDiscreteTimeRoots);
+
+    // Obtain the coefficients of numerator and denominator
+    i = 0;
+    mDiscreteTimeNumerator = polynomialFromRoots(mDiscreteTimeZeros);
+
+    for (auto &&dn : mDiscreteTimeNumerator)
+    {
+      dn = dn * mDiscreteTimeGain;
+      mBn[i] = dn.real();
+      i++;
+    }
+
+    i = 0;
+    for (auto &&dd : mDiscreteTimeDenominator)
+    {
+      mAn[i] = dd.real();
+      i++;
+    }
+  }
+}
+
+Order_Cutoff ButterworthFilter::getOrderCutOff()
+{
+  Order_Cutoff NWc{mOrder, mCutoff_Frequency};
+
+  return NWc;
+}
+
+DifferenceAnBn ButterworthFilter::getAnBn()
+{
+  //    DifferenceAnBn AnBn;
+  //    AnBn.An.resize(mAn.size(), 0.0);
+  //    AnBn.Bn.resize(mBn.size(), 0.0);
+  //
+  //    AnBn.An = mAn;
+  //    AnBn.Bn = mBn;
+
+  DifferenceAnBn AnBn{mAn, mBn};
+
+  return AnBn;
+}
+
+std::vector<double> ButterworthFilter::getAn()
+{
+  return mAn;
+}
+
+std::vector<double> ButterworthFilter::getBn()
+{
+  return mBn;
+}
+
+void ButterworthFilter::PrintFilter_Specs()
+{
+  /*
+   * Prints the order and cut-off angular frequency (rad/sec) of the filter
+   *
+   * */
+  std::cout << "\nThe order of the filter : " << this->mOrder << std::endl;
+  std::cout << "Cut-off Frequency : " << this->mCutoff_Frequency << " rad/sec\n"
+            << std::endl;
+}
+
+void ButterworthFilter::PrintFilter_ContinuousTimeRoots()
+{
+  /*
+   * Prints the order and cut-off angular frequency (rad/sec) of the filter
+   * */
+  std::cout << "\n Roots of Continous Time Filter Transfer Function "
+               "Denominator are : "
+            << std::endl;
+
+  for (auto &&x : mContinuousTimeRoots)
+  {
+    std::cout << std::real(x) << " + j " << std::imag(x) << std::endl;
+  }
+  std::cout << std::endl;
+}
+
+void ButterworthFilter::PrintContinuousTimeTF()
+{
+  int n = mOrder;
+
+  std::cout << "\nThe Continuous Time Transfer Function of the Filter is ;\n"
+            << std::endl;
+
+  std::cout << "         " << mContinuousTimeNumerator << std::endl;
+
+  for (int i = 0; i <= n; i++)
+  {
+    std::cout << "--------";
+  }
+
+  std::cout << "--------\n";
+
+  for (int i = n; i > 0; i--)
+  {
+    std::cout << mContinuousTimeDenominator[n - i].real() << " * ";
+    std::cout << "z[-" << i << "] + ";
+  }
+
+  std::cout << mContinuousTimeDenominator[n].real() << std::endl;
+}
+
+void ButterworthFilter::PrintDiscreteTimeTF()
+{
+  int n = mOrder;
+  std::cout << "\nThe Discrete Time Transfer Function of the Filter is ;\n"
+            << std::endl;
+
+  for (int i = n; i > 0; i--)
+  {
+    std::cout << mDiscreteTimeNumerator[n - i].real() << " * ";
+    std::cout << "z[-" << i << "] + ";
+  }
+
+  std::cout << mDiscreteTimeNumerator[n].real() << std::endl;
+
+  for (int i = 0; i <= n; i++)
+  {
+    std::cout << "--------";
+  }
+
+  std::cout << "--------\n";
+
+  for (int i = n; i > 0; i--)
+  {
+    std::cout << mDiscreteTimeDenominator[n - i].real() << " * ";
+    std::cout << "z[-" << i << "] + ";
+  }
+
+  std::cout << mDiscreteTimeDenominator[n].real() << std::endl;
+  std::cout << std::endl;
+}
+
+double ButterworthFilter::filter(const double &u)
+{
+  double u_f = 0.0;
+
+  for (int i = 0; i < mOrder + 1; i++)
+  {
+    if (i == 0)
+    {
+      u_f += mBn[0] * u;
+    }
+    else
+    {
+      u_f += mBn[i] * u_unfiltered[i];
+      u_f -= mAn[i] * u_filtered[i];
+    }
+  }
+
+  for (int i = mOrder; i >= 2; i--)
+  {
+    u_unfiltered[i] = u_unfiltered[i - 1];
+    u_filtered[i] = u_filtered[i - 1];
+  }
+  u_unfiltered[1] = u;
+  u_filtered[1] = u_f;
+
+  return u_f;
+}
+
+void ButterworthFilter::filtVector(const std::vector<double> &t,
+                                   std::vector<double> &u,
+                                   bool init_first_value)
+{
+  // initialise trace values to 1st value
+
+  if (init_first_value)
+  {
+    for (int i = 0; i < mOrder + 1; i++)
+    {
+      u_unfiltered[i] = t[0];
+      u_filtered[i] = t[0];
+    }
+  }
+
+  for (unsigned int i = 0; i < t.size(); ++i)
+    u[i] = this->filter(t[i]);
+}
+
+void ButterworthFilter::filtFiltVector(const std::vector<double> &t,
+                                       std::vector<double> &u,
+                                       bool init_first_value)
+{
+  std::vector<double> u_rev(u);
+
+  // forward filtering
+  filtVector(t, u, init_first_value);
+
+  // backward filtering
+  std::reverse(u_rev.begin(), u_rev.end());
+  filtVector(t, u_rev, init_first_value);
+  std::reverse(u_rev.begin(), u_rev.end());
+
+  // merge
+  for (unsigned int i = 0; i < u.size(); ++i)
+  {
+    u[i] = (u[i] + u_rev[i]) * 0.5;
+  }
+}

src/ros/catkin/src/amathutils_lib/src/kalman_filter.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#include "amathutils_lib/kalman_filter.hpp"
+
+KalmanFilter::KalmanFilter() {}
+KalmanFilter::KalmanFilter(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+                           const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+                           const Eigen::MatrixXd &P)
+{
+  init(x, A, B, C, Q, R, P);
+}
+KalmanFilter::~KalmanFilter() {}
+bool KalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &A,
+                        const Eigen::MatrixXd &B, const Eigen::MatrixXd &C,
+                        const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
+                        const Eigen::MatrixXd &P)
+{
+  if (x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 ||
+      B.cols() == 0 || B.rows() == 0 || C.cols() == 0 || C.rows() == 0 ||
+      Q.cols() == 0 || Q.rows() == 0 || R.cols() == 0 || R.rows() == 0 ||
+      P.cols() == 0 || P.rows() == 0)
+  {
+    return false;
+  }
+  x_ = x;
+  A_ = A;
+  B_ = B;
+  C_ = C;
+  Q_ = Q;
+  R_ = R;
+  P_ = P;
+  return true;
+}
+bool KalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0)
+{
+  if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0)
+  {
+    return false;
+  }
+  x_ = x;
+  P_ = P0;
+  return true;
+}
+
+void KalmanFilter::setA(const Eigen::MatrixXd &A) { A_ = A; }
+void KalmanFilter::setB(const Eigen::MatrixXd &B) { B_ = B; }
+void KalmanFilter::setC(const Eigen::MatrixXd &C) { C_ = C; }
+void KalmanFilter::setQ(const Eigen::MatrixXd &Q) { Q_ = Q; }
+void KalmanFilter::setR(const Eigen::MatrixXd &R) { R_ = R; }
+void KalmanFilter::getX(Eigen::MatrixXd &x) { x = x_; }
+void KalmanFilter::getP(Eigen::MatrixXd &P) { P = P_; }
+double KalmanFilter::getXelement(unsigned int i) { return x_(i); }
+
+bool KalmanFilter::predict(const Eigen::MatrixXd &x_next,
+                           const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &Q)
+{
+  if (x_.rows() != x_next.rows() || A.cols() != P_.rows() ||
+      Q.cols() != Q.rows() || A.rows() != Q.cols())
+  {
+    return false;
+  }
+  x_ = x_next;
+  P_ = A * P_ * A.transpose() + Q;
+  return true;
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd &x_next,
+                           const Eigen::MatrixXd &A)
+{
+  return predict(x_next, A, Q_);
+}
+
+bool KalmanFilter::predict(const Eigen::MatrixXd &u, const Eigen::MatrixXd &A,
+                           const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q)
+{
+  if (A.cols() != x_.rows() || B.cols() != u.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd x_next = A * x_ + B * u;
+  return predict(x_next, A, Q);
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd &u) { return predict(u, A_, B_, Q_); }
+
+bool KalmanFilter::update(const Eigen::MatrixXd &y,
+                          const Eigen::MatrixXd &y_pred,
+                          const Eigen::MatrixXd &C,
+                          const Eigen::MatrixXd &R)
+{
+  if (P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() ||
+      y.rows() != y_pred.rows() || y.rows() != C.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd PCT = P_ * C.transpose();
+  const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
+
+  if (isnan(K.array()).any() || isinf(K.array()).any())
+  {
+    return false;
+  }
+
+  x_ = x_ + K * (y - y_pred);
+  P_ = P_ - K * (C * P_);
+  return true;
+}
+
+bool KalmanFilter::update(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                          const Eigen::MatrixXd &R)
+{
+  if (C.cols() != x_.rows())
+  {
+    return false;
+  }
+  const Eigen::MatrixXd y_pred = C * x_;
+  return update(y, y_pred, C, R);
+}
+bool KalmanFilter::update(const Eigen::MatrixXd &y) { return update(y, C_, R_); }

src/ros/catkin/src/amathutils_lib/src/time_delay_kalman_filter.cpp

@@ -0,0 +1,97 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#include "amathutils_lib/time_delay_kalman_filter.hpp"
+
+TimeDelayKalmanFilter::TimeDelayKalmanFilter() {}
+
+void TimeDelayKalmanFilter::init(const Eigen::MatrixXd &x, const Eigen::MatrixXd &P0,
+                                 const int max_delay_step)
+{
+  max_delay_step_ = max_delay_step;
+  dim_x_ = x.rows();
+  dim_x_ex_ = dim_x_ * max_delay_step;
+
+  x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+
+  for (int i = 0; i < max_delay_step_; ++i)
+  {
+    x_.block(i * dim_x_, 0, dim_x_, 1) = x;
+    P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0;
+  }
+}
+
+void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd &x) { x = x_.block(0, 0, dim_x_, 1); }
+void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd &P) { P = P_.block(0, 0, dim_x_, dim_x_); }
+
+bool TimeDelayKalmanFilter::predictWithDelay(const Eigen::MatrixXd &x_next, const Eigen::MatrixXd &A,
+                                             const Eigen::MatrixXd &Q)
+{
+/*
+ * time delay model:
+ * 
+ *     [A   0   0]      [P11   P12   P13]      [Q   0   0]
+ * A = [I   0   0], P = [P21   P22   P23], Q = [0   0   0]
+ *     [0   I   0]      [P31   P32   P33]      [0   0   0]
+ * 
+ * covariance calculation in prediction : P = A * P * A' + Q
+ *
+ *     [A*P11*A'*+Q  A*P11  A*P12]
+ * P = [     P11*A'    P11    P12]
+ *     [     P21*A'    P21    P22]
+ */
+
+  const int d_dim_x = dim_x_ex_ - dim_x_;
+
+  /* slide states in the time direction */
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  x_tmp.block(0, 0, dim_x_, 1) = x_next;
+  x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1);
+  x_ = x_tmp;
+
+  /* update P with delayed measurement A matrix structure */
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+  P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q;
+  P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x);
+  P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose();
+  P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x);
+  P_ = P_tmp;
+
+  return true;
+}
+
+bool TimeDelayKalmanFilter::updateWithDelay(const Eigen::MatrixXd &y, const Eigen::MatrixXd &C,
+                                            const Eigen::MatrixXd &R, const int delay_step)
+{
+  if (delay_step >= max_delay_step_)
+  {
+    std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl;
+    return false;
+  }
+
+  const int dim_y = y.rows();
+
+  /* set measurement matrix */
+  Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_);
+  C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C;
+
+  /* update */
+  if (!update(y, C_ex, R))
+    return false;
+
+  return true;
+}

src/ros/catkin/src/amathutils_lib/test/src/test_amathutils_lib.cpp

@@ -0,0 +1,379 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#include "amathutils_lib/amathutils.hpp"
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+
+class TestSuite:
+  public ::testing::Test
+{
+public:
+  TestSuite() {}
+};
+
+TEST(TestSuite, Rad2Deg)
+{
+  double radAngle = 1;
+  ASSERT_EQ(amathutils::rad2deg(radAngle), radAngle*180/M_PI) << "Angle in degrees should be " << radAngle*180/M_PI;
+}
+
+TEST(TestSuite, Deg2Rad)
+{
+  double degAngle = 10;
+  ASSERT_EQ(amathutils::deg2rad(degAngle), degAngle*M_PI/180) << "Angle in radians should be " << degAngle*M_PI/180;
+}
+
+// Value from https://www.google.com/search?client=ubuntu&channel=fs&q=mps+to+kph&ie=utf-8&oe=utf-8
+TEST(TestSuite, Transform_mps2kph)
+{
+  double mpsValue = 1;
+  ASSERT_DOUBLE_EQ(amathutils::mps2kmph(mpsValue), mpsValue*3.6) << "Speed should be " << mpsValue*3.6 << "kmph";
+}
+
+// Value from https://www.google.com/search?client=ubuntu&channel=fs&q=kph+to+mps&ie=utf-8&oe=utf-8
+TEST(TestSuite, Transform_kmph2mps)
+{
+  double kmphValue = 1;
+  ASSERT_DOUBLE_EQ(amathutils::kmph2mps(kmphValue), kmphValue/3.6) << "Speed should be " << kmphValue/3.6 << "mps";
+}
+
+TEST(TestSuite, GetGravityAcceleration)
+{
+  double accel = 10.8;
+  ASSERT_DOUBLE_EQ(amathutils::getGravityAcceleration(accel), accel/G_MPSS)
+    << "Acceleration should be " << accel/G_MPSS << "g";
+}
+
+TEST(TestSuite, GetAcceleration)
+{
+  double accel = 5.5;
+  double t = 2;
+  double v0 = 2.5;
+  double v = v0 + accel*t;
+  double x = v0*t + 0.5*accel*t*t;
+  ASSERT_DOUBLE_EQ(amathutils::getAcceleration(v0, v, x), accel) << "Acceleration should be " << accel;
+}
+
+TEST(TestSuite, GetTimeFromAcceleration)
+{
+  double accel = 0.5;
+  double t = 2.8;
+  double v0 = 2;
+  double v = v0 + accel*t;
+  ASSERT_DOUBLE_EQ(amathutils::getTimefromAcceleration(v0, v, accel), t) << "Time should be " << t;
+}
+
+// Values taken from https://gerardnico.com/linear_algebra/closest_point_line - Example 5.4
+TEST(TestSuite, GetNearPointOnLine2D)
+{
+  geometry_msgs::Point a, b, p, nearPOut, nearP;
+
+  a.x = 0;
+  a.y = 0;
+  a.z = 0;
+
+  b.x = 6;
+  b.y = 2;
+  b.z = 0;
+
+  p.x = 2;
+  p.y = 4;
+  p.z = 0;
+
+  nearP.x = 3;
+  nearP.y = 1;
+  nearP.z = 0;
+
+  nearPOut = amathutils::getNearPtOnLine(p, a, b);
+
+  ASSERT_DOUBLE_EQ(nearPOut.x, nearP.x) << "nearPoint coordinate X should be " << nearP.x;
+  ASSERT_DOUBLE_EQ(nearPOut.y, nearP.y) << "nearPoint coordinate X should be " << nearP.y;
+  ASSERT_DOUBLE_EQ(nearPOut.z, nearP.z) << "nearPoint coordinate X should be " << nearP.z;
+}
+
+// Values taken from https://math.stackexchange.com/questions/13176/how-to-find-a-point-on-a-line-closest-to-another-given-point
+TEST(TestSuite, GetNearPointOnLine3D)
+{
+  geometry_msgs::Point a, b, p, nearPOut, nearP;
+  double threshold = 0.00000001;
+
+  a.x = -2;
+  a.y = -4;
+  a.z = 5;
+
+  b.x = 0;
+  b.y = 0;
+  b.z = 1;
+
+  p.x = 1;
+  p.y = 1;
+  p.z = 1;
+
+  nearP.x = 1.0/3.0;
+  nearP.y = 2.0/3.0;
+  nearP.z = 1.0/3.0;
+
+  nearPOut = amathutils::getNearPtOnLine(p, a, b);
+
+  ASSERT_NEAR(nearPOut.x, nearP.x, threshold) << "nearPoint coordinate X should be " << nearP.x;
+  ASSERT_NEAR(nearPOut.y, nearP.y, threshold) << "nearPoint coordinate X should be " << nearP.y;
+  ASSERT_NEAR(nearPOut.z, nearP.z, threshold) << "nearPoint coordinate X should be " << nearP.z;
+}
+
+// Values from http://www.math.usm.edu/lambers/mat169/fall09/lecture17.pdf - The Distance Formula
+TEST(TestSuite, GetDistance)
+{
+  geometry_msgs::Point ptA, ptB;
+  geometry_msgs::Pose poseA, poseB;
+
+  ptA.x = 2;
+  ptA.y = 3;
+  ptA.z = 1;
+
+  ptB.x = 8;
+  ptB.y = -5;
+  ptB.z = 0;
+
+  poseA.position.x = 2;
+  poseA.position.y = 3;
+  poseA.position.z = 1;
+
+  poseB.position.x = 8;
+  poseB.position.y = -5;
+  poseB.position.z = 0;
+
+  ASSERT_DOUBLE_EQ(amathutils::find_distance(ptA, ptB), sqrt(101))
+    << "Distance between points should be " << sqrt(101);
+  ASSERT_DOUBLE_EQ(amathutils::find_distance(poseA, poseB), sqrt(101))
+    << "Distance between poses should be " << sqrt(101);
+}
+
+// Values from https://math.stackexchange.com/questions/707673/find-angle-in-degrees-from-one-point-to-another-in-2d-space
+TEST(TestSuite, GetAngle)
+{
+  geometry_msgs::Point ptA, ptB;
+
+  ptA.x = 0;
+  ptA.y = 10;
+  ptA.z = 0;
+
+  ptB.x = 10;
+  ptB.y = 20;
+  ptB.z = 0;
+
+  ASSERT_DOUBLE_EQ(amathutils::find_angle(ptA, ptB), 45) << "Angle should be 45deg";
+  ASSERT_DOUBLE_EQ(amathutils::find_angle(ptB, ptA), 225) << "Angle should be -45deg";
+}
+
+// Values from https://www.mathopenref.com/coordintersection.html
+TEST(TestSuite, LineIntersect)
+{
+  geometry_msgs::Point l1_p1, l1_p2, l2_p1, l2_p2;
+  l1_p1.x = 29;
+  l1_p1.y = 5;
+  l1_p1.z = 0;
+  l1_p2.x = 51;
+  l1_p2.y = 15;
+  l1_p2.z = 0;
+  l2_p1.x = 15;
+  l2_p1.y = 10;
+  l2_p1.z = 0;
+  l2_p2.x = 58;
+  l2_p2.y = 10;
+  l2_p2.z = 0;
+
+  ASSERT_TRUE(amathutils::isIntersectLine(l1_p1, l1_p2, l2_p1, l2_p2)) << "Lines intersect";
+}
+
+// Values from https://www.mathopenref.com/coordintersection.html
+TEST(TestSuite, ParallelLines)
+{
+  geometry_msgs::Point l1_p1, l1_p2, l2_p1, l2_p2;
+  l1_p1.x = 29;
+  l1_p1.y = 5;
+  l1_p1.z = 0;
+  l1_p2.x = 51;
+  l1_p2.y = 15;
+  l1_p2.z = 0;
+  l2_p1.x = 15;
+  l2_p1.y = 10;
+  l2_p1.z = 0;
+  l2_p2.x = 49;
+  l2_p2.y = 25;
+  l2_p2.z = 0;
+
+  ASSERT_TRUE(!amathutils::isIntersectLine(l1_p1, l1_p2, l2_p1, l2_p2)) << "Parallel lines";
+}
+
+#define LEFT 1
+#define RIGHT -1
+#define ONLINE 0
+
+// 45degree angle line through the origin (0,0)
+TEST(TestSuite, PointOnTheLeft)
+{
+  geometry_msgs::Point p1, line_p1, line_p2;
+  p1.x = 1;
+  p1.y = 10;
+  p1.z = 0;
+  line_p1.x = 1;
+  line_p1.y = 1;
+  line_p1.z = 0;
+  line_p2.x = 10;
+  line_p2.y = 10;
+  line_p2.z = 0;
+
+  ASSERT_EQ(amathutils::isPointLeftFromLine(p1, line_p1, line_p2), LEFT) << "Point is on the left";
+}
+
+// 45degree angle line through the origin (0,0)
+TEST(TestSuite, PointOnTheRight)
+{
+  geometry_msgs::Point p1, line_p1, line_p2;
+  p1.x = 10;
+  p1.y = 1;
+  p1.z = 0;
+  line_p1.x = 1;
+  line_p1.y = 1;
+  line_p1.z = 0;
+  line_p2.x = 10;
+  line_p2.y = 10;
+  line_p2.z = 0;
+
+  ASSERT_EQ(amathutils::isPointLeftFromLine(p1, line_p1, line_p2), RIGHT) << "Point is on the right";
+}
+
+// 45degree angle line through the origin (0,0)
+TEST(TestSuite, PointOnTheLine)
+{
+  geometry_msgs::Point p1, line_p1, line_p2;
+  p1.x = -5;
+  p1.y = -5;
+  p1.z = 0;
+  line_p1.x = 1;
+  line_p1.y = 1;
+  line_p1.z = 0;
+  line_p2.x = 10;
+  line_p2.y = 10;
+  line_p2.z = 0;
+
+  ASSERT_EQ(amathutils::isPointLeftFromLine(p1, line_p1, line_p2), ONLINE) << "Point is on the line ";
+}
+// Point between p1, p2
+TEST(TestSuite, DistanceFromSegment1)
+{
+  geometry_msgs::Point p, line_p1, line_p2;
+  p.x = 1;
+  p.y = 1;
+  p.z = 0;
+  line_p1.x = 0;
+  line_p1.y = 0;
+  line_p1.z = 0;
+  line_p2.x = 2;
+  line_p2.y = 0;
+  line_p2.z = 0;
+
+  ASSERT_DOUBLE_EQ(amathutils::distanceFromSegment(line_p1, line_p2, p), 1.0) << "Point is 1m away from segment";
+}
+
+// Point 2m away from p1
+TEST(TestSuite, DistanceFromSegment2)
+{
+  geometry_msgs::Point p, line_p1, line_p2;
+  p.x = -2;
+  p.y = 0;
+  p.z = 0;
+  line_p1.x = 0;
+  line_p1.y = 0;
+  line_p1.z = 0;
+  line_p2.x = 2;
+  line_p2.y = 0;
+  line_p2.z = 0;
+
+  ASSERT_DOUBLE_EQ(amathutils::distanceFromSegment(line_p1, line_p2, p), 2.0) << "Point is 2m away from p1";
+}
+
+// Point 3m away from p2
+TEST(TestSuite, DistanceFromSegment3)
+{
+  geometry_msgs::Point p, line_p1, line_p2;
+  p.x = 5;
+  p.y = 0;
+  p.z = 0;
+  line_p1.x = 0;
+  line_p1.y = 0;
+  line_p1.z = 0;
+  line_p2.x = 2;
+  line_p2.y = 0;
+  line_p2.z = 0;
+
+  ASSERT_DOUBLE_EQ(amathutils::distanceFromSegment(line_p1, line_p2, p), 3.0) << "Point is 3m away from p1";
+}
+
+TEST(TestSuite, TestYawQuaternion)
+{
+  geometry_msgs::Quaternion q;
+  q = amathutils::getQuaternionFromYaw(0.0);
+  ASSERT_DOUBLE_EQ(0.0, q.x);
+  ASSERT_DOUBLE_EQ(0.0, q.y);
+  ASSERT_DOUBLE_EQ(0.0, q.z);
+  ASSERT_DOUBLE_EQ(1.0, q.w);
+}
+
+TEST(TestSuite, TestNormalizeRadian)
+{
+  ASSERT_DOUBLE_EQ(0.0, amathutils::normalizeRadian(0.0));
+  ASSERT_DOUBLE_EQ(0.5, amathutils::normalizeRadian(0.5));
+  ASSERT_DOUBLE_EQ(-0.5, amathutils::normalizeRadian(-0.5));
+  ASSERT_DOUBLE_EQ(0.0, amathutils::normalizeRadian(2.0 * M_PI));
+  ASSERT_DOUBLE_EQ(0.3, amathutils::normalizeRadian(2.0 * M_PI + 0.3));
+  ASSERT_DOUBLE_EQ(M_PI, amathutils::normalizeRadian(3.0 * M_PI));
+  ASSERT_DOUBLE_EQ(0.0, amathutils::normalizeRadian(4.0 * M_PI));
+  ASSERT_DOUBLE_EQ(0.0, amathutils::normalizeRadian(-2.0 * M_PI));
+  ASSERT_DOUBLE_EQ(0.3, amathutils::normalizeRadian(-2.0 * M_PI + 0.3));
+  ASSERT_DOUBLE_EQ(-M_PI, amathutils::normalizeRadian(-3.0 * M_PI));
+  ASSERT_DOUBLE_EQ(0.0, amathutils::normalizeRadian(-4.0 * M_PI));
+  ASSERT_DOUBLE_EQ(M_PI, amathutils::normalizeRadian(M_PI));
+  ASSERT_DOUBLE_EQ(-M_PI, amathutils::normalizeRadian(-M_PI));
+}
+
+TEST(TestSuite, TestGetIntersect)
+{
+  geometry_msgs::Point line1_p1, line1_p2, line2_p1, line2_p2, intersect_p;
+  line1_p1.x = 5;
+  line1_p1.y = 0;
+  line1_p2.x = 5;
+  line1_p2.y = 10;
+  line2_p1.x = 0;
+  line2_p1.y = 5;
+  line2_p2.x = 10;
+  line2_p2.y = 5;
+
+  ASSERT_TRUE(amathutils::getIntersect(line1_p1, line1_p2, line2_p1, line2_p2, &intersect_p));
+  ASSERT_DOUBLE_EQ(5, intersect_p.x);
+  ASSERT_DOUBLE_EQ(5, intersect_p.y);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/amathutils_lib/test/src/test_kalman_filter.cpp

@@ -0,0 +1,166 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+
+#include "amathutils_lib/kalman_filter.hpp"
+#include "amathutils_lib/time_delay_kalman_filter.hpp"
+
+class KalmanFilterTestSuite :
+  public ::testing::Test
+{
+public:
+  KalmanFilterTestSuite() {}
+};
+
+TEST_F(KalmanFilterTestSuite, updateCase)
+{
+  KalmanFilter kf;
+  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd y = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd P = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);
+  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd R = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd y_pred = y;
+
+  ASSERT_EQ(false, kf.update(y)) << "uninitialized, false expected";
+  ASSERT_EQ(false, kf.update(y, C, R)) << "uninitialized, false expected";
+  ASSERT_EQ(false, kf.update(y, y_pred, C, R)) << "uninitialized, false expected";
+
+  kf.init(x, P);
+  Eigen::MatrixXd no_initialized;
+  ASSERT_EQ(false, kf.update(no_initialized)) << "inappropriate argument, false expected";
+
+  Eigen::MatrixXd R0 = Eigen::MatrixXd::Zero(3, 3);
+  Eigen::MatrixXd P0 = Eigen::MatrixXd::Zero(3, 3);
+  kf.init(x, P0);
+  ASSERT_EQ(false, kf.update(y, y_pred, C, R0)) << "R0 inverse problem, false expected";
+
+  kf.init(x, P);
+  Eigen::MatrixXd R_bad_dim = Eigen::MatrixXd::Identity(4, 4);
+  ASSERT_EQ(false, kf.update(y, y_pred, C, R_bad_dim)) << "R dimension problem, false expected";
+
+  Eigen::MatrixXd y_bad_dim = Eigen::MatrixXd::Identity(4, 1);
+  ASSERT_EQ(false, kf.update(y, y_bad_dim, C, R_bad_dim)) << "y_pred dimension problem, false expected";
+  ASSERT_EQ(false, kf.update(y_bad_dim, y_pred, C, R_bad_dim)) << "y dimension problem, false expected";
+
+  Eigen::MatrixXd C_bad_dim = Eigen::MatrixXd::Identity(2, 2);
+  ASSERT_EQ(false, kf.update(y, y_pred, C_bad_dim, R_bad_dim)) << "C dimension problem, false expected";
+  ASSERT_EQ(false, kf.update(y, y_pred, C_bad_dim)) << "C dimension problem, false expected";
+
+  kf.init(x, P);
+  ASSERT_EQ(true, kf.update(y, y_pred, C, R)) << "normal process, true expected";
+  ASSERT_EQ(true, kf.update(y, C, R)) << "normal process, true expected";
+
+  kf.init(x, P);
+  y << 1.0, 1.0, 1.0;
+  y_pred << 0.0, 0.0, 0.0;
+  kf.update(y, y_pred, C, R);
+  Eigen::MatrixXd X_expected(3, 1);
+  X_expected << 0.5, 0.5, 0.5;
+  Eigen::MatrixXd X_actual;
+  kf.getX(X_actual);
+  ASSERT_TRUE((X_actual - X_expected).norm() < 1.0E-6) << "X_actual^T : "
+    << X_actual.transpose() << ", X_expected^T : " << X_expected.transpose();
+}
+
+TEST_F(KalmanFilterTestSuite, predictCase)
+{
+  KalmanFilter kf;
+  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd x_next = x;
+  Eigen::MatrixXd u = Eigen::MatrixXd::Zero(2, 1);
+  Eigen::MatrixXd P = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);
+  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(3, 3);
+
+  ASSERT_EQ(false, kf.predict(u)) << "uninitialized, false expected";
+  ASSERT_EQ(false, kf.predict(x_next, A)) << "uninitialized, false expected";
+
+  kf.init(x, P);
+  Eigen::MatrixXd no_initialized;
+  ASSERT_EQ(false, kf.predict(no_initialized, A, Q)) << "inappropriate argument, false expected";
+  ASSERT_EQ(false, kf.predict(x_next, no_initialized, Q)) << "inappropriate argument, false expected";
+  ASSERT_EQ(false, kf.predict(x_next, A, no_initialized)) << "inappropriate argument, false expected";
+  ASSERT_EQ(false, kf.predict(no_initialized, no_initialized, no_initialized))
+    << "inappropriate argument, false expected";
+  ASSERT_EQ(false, kf.init(x, no_initialized)) << "inappropriate argument, false expected";
+
+  Eigen::MatrixXd A_bad_dim = Eigen::MatrixXd::Zero(4, 4);
+  ASSERT_EQ(false, kf.predict(x_next, A_bad_dim, Q)) << "A dimension problem, false expected";
+  Eigen::MatrixXd Q_bad_dim = Eigen::MatrixXd::Zero(4, 4);
+  ASSERT_EQ(false, kf.predict(x_next, A, Q_bad_dim)) << "Q dimension problem, false expected";
+  Eigen::MatrixXd x_bad_dim = Eigen::MatrixXd::Zero(4, 1);
+  ASSERT_EQ(false, kf.predict(x_bad_dim, A, Q)) << "x dimension problem, false expected";
+
+  ASSERT_EQ(true, kf.predict(x_next, A, Q)) << "normal process, true expected";
+  ASSERT_EQ(true, kf.predict(u, A, B, Q)) << "normal process, true expected";
+
+  kf.init(x, P);
+  A = Eigen::MatrixXd::Identity(3, 3);
+  Q = Eigen::MatrixXd::Identity(3, 3);
+  kf.predict(x_next, A, Q);
+  Eigen::MatrixXd P_expected = Eigen::MatrixXd::Identity(3, 3) * 2.0;
+  Eigen::MatrixXd P_actual;
+  kf.getP(P_actual);
+  ASSERT_TRUE((P_actual - P_expected).norm() < 1.0E-6) << "P_actual : "
+    << P_actual << ", P_expected : " << P_expected;
+}
+
+TEST_F(KalmanFilterTestSuite, delayedMeasurement)
+{
+  TimeDelayKalmanFilter tdkf;
+  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd x_next = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd y = Eigen::MatrixXd::Zero(3, 1);
+  Eigen::MatrixXd P = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd A = Eigen::MatrixXd::Identity(3, 3) * 2.0;
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Identity(3, 3);
+  Eigen::MatrixXd R = Eigen::MatrixXd::Identity(3, 3);
+  int max_delay_step = 5;
+
+  tdkf.init(x, P, max_delay_step);
+
+  x_next << 1.0, 2.0, 3.0;
+  tdkf.predictWithDelay(x_next, A, Q);
+  Eigen::MatrixXd x_curr;
+  tdkf.getLatestX(x_curr);
+  ASSERT_TRUE((x_next - x_curr).norm() < 1.0E-6);
+
+  int delay_step = max_delay_step - 1;
+  ASSERT_EQ(true, tdkf.updateWithDelay(y, C, R, delay_step));
+
+  delay_step = max_delay_step;
+  ASSERT_EQ(false, tdkf.updateWithDelay(y, C, R, delay_step));
+
+  delay_step = max_delay_step + 1;
+  ASSERT_EQ(false, tdkf.updateWithDelay(y, C, R, delay_step));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "KalmanFilterTestSuite");
+  ros::NodeHandle nh;
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/amathutils_lib/test/test_amathutils_lib.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-amathutils" pkg="amathutils_lib" type="amathutils-test" name="test"/>
+
+</launch>

src/ros/catkin/src/amathutils_lib/test/test_butterworth_filter.test

@@ -0,0 +1,5 @@
+  <launch>
+
+  <test test-name="test-butterworth_filter" pkg="amathutils_lib" type="test-butterworth_filter" name="test"/>
+
+</launch>

src/ros/catkin/src/amathutils_lib/test/test_kalman_filter.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-kalman_filter" pkg="amathutils_lib" type="test-kalman_filter" name="test"/>
+
+</launch>

src/ros/catkin/src/autoware_build_flags/CHANGELOG.rst

@@ -0,0 +1,23 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_build_flags
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* Fix license notice in corresponding package.xml
+* Contributors: amc-nu
+
+1.10.0 (2019-01-17)
+-------------------
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+
+1.8.0 (2018-08-31)
+------------------
+* [Fix] Moved C++11 flag to autoware_build_flags (`#1395 <https://github.com/CPFL/Autoware/pull/1395>`_)
+* [Feature] Makes sure that all binaries have their dependencies linked (`#1385 <https://github.com/CPFL/Autoware/pull/1385>`_)
+* Contributors: Esteve Fernandez

src/ros/catkin/src/autoware_build_flags/CMakeLists.txt

@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_build_flags)
+
+find_package(catkin REQUIRED)
+
+catkin_package(CFG_EXTRAS autoware_build_flags-extras.cmake)

src/ros/catkin/src/autoware_build_flags/cmake/autoware_build_flags-extras.cmake

@@ -0,0 +1,39 @@
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--no-undefined")
+elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined,error")
+endif()
+
+# Enable support for C++14
+if(${CMAKE_VERSION} VERSION_LESS "3.1.0")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+else()
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+message(STATUS "CUDA compilation status: $ENV{AUTOWARE_COMPILE_WITH_CUDA}.")
+
+macro(AW_CHECK_CUDA)
+  if ($ENV{AUTOWARE_COMPILE_WITH_CUDA})
+    find_package(CUDA REQUIRED)
+    find_package(Eigen3 REQUIRED)
+
+    if(NOT ${CUDA_VERSION} VERSION_LESS "10.0"
+            AND NOT ${CUDA_VERSION} VERSION_EQUAL "10.0" )
+      message(FATAL_ERROR "GPU support on Melodic requires CUDA<=10.0")
+    endif()
+    if(${CUDA_VERSION} VERSION_GREATER "9.1"
+          AND ${CMAKE_VERSION} VERSION_LESS "3.12.3")
+      unset(CUDA_cublas_device_LIBRARY CACHE)
+      set(CUDA_cublas_device_LIBRARY ${CUDA_cublas_LIBRARY})
+      set(CUDA_CUBLAS_LIBRARIES ${CUDA_cublas_LIBRARY})
+    endif()
+    if ("$ENV{ROS_DISTRO}" STREQUAL "melodic" AND ${EIGEN3_VERSION_STRING} VERSION_LESS "3.3.7")
+      message(FATAL_ERROR "GPU support on Melodic requires Eigen version>= 3.3.7")
+    endif()
+    set(USE_CUDA ON)
+  else()
+    message(WARNING "CUDA support is disabled. Set the AUTOWARE_COMPILE_WITH_CUDA environment variable and recompile to enable it")
+    set(USE_CUDA OFF)
+  endif()
+endmacro()

src/ros/catkin/src/autoware_build_flags/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autoware_build_flags</name>
+  <version>1.12.0</version>
+  <description>Common build flags for Autoware.</description>
+
+  <author email="esteve@apache.org">Esteve Fernandez</author>
+  <maintainer email="esteve@apache.org">Esteve Fernandez</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export>
+    <architecture_independent/>
+  </export>
+</package>

src/ros/catkin/src/autoware_config_msgs/CHANGELOG.rst

@@ -0,0 +1,328 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.0 (2019-12-03)
+-------------------
+* Lateral limitation filter by twist_filter
+* add param of decision_maker: disable insert stopline
+* Add Message Properties for use_sim and use_decision_maker
+* Update package.xml files to Format 2.
+* Contributors: Joshua Whitley, Yuma Nihei, s-azumi
+
+1.12.0 (2019-07-12)
+-------------------
+
+1.11.0 (2019-03-21)
+-------------------
+* [Feature] Rebuild decision maker (`#1609 <https://github.com/CPFL/Autoware/issues/1609>`_)
+* [Feature] waypoint replanner and extractor (`#1951 <https://github.com/CPFL/Autoware/issues/1951>`_)
+* Fix license notice in corresponding package.xml
+* Contributors: Yuma Nihei, amc-nu, s-azumi
+
+1.10.0 (2019-01-17)
+-------------------
+* Fix Kf/KF naming convention
+* Fix Dpm/DPM naming convention
+* Fix Ndt/NDT naming convention
+* Fix Ssd/SSD naming convention
+* Contributors: Esteve Fernandez
+
+1.9.1 (2018-11-06)
+------------------
+
+1.9.0 (2018-10-31)
+------------------
+* Moved configuration messages to autoware_config_msgs
+* Contributors: Esteve Fernandez
+
+1.7.0 (2018-05-18)
+------------------
+* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst
+* [Fix] rename packages (`#1269 <https://github.com/CPFL/Autoware/issues/1269>`_)
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * Rename road_wizard to trafficlight_recognizer
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * removed unnecessary comments
+  * rename lidar_tracker
+  * Modify pf_lidar_track's cmake file
+  * Refactor code
+  * Rename from euclidean_lidar_tracker to lidar_euclidean_track
+  * Rename from kf_contour_track to lidar_kf_contour_track
+  * Rename from kf_lidar_track to lidar_kf_track, but need some modification in euclidean cluster(Cluster.h)
+  * Rename from pf_lidar_tarck to lidar_pf_track
+  * Rename range_fusion
+  * Rename obj_reproj
+  * Rename road_wizard to trafficlight_recognizer
+  * Rename euclidean_cluster to lidar_euclidean_cluster_detect
+  * Rename svm_lidar_detect to lidar_svm_detect
+  * Rename kf_lidar_track to lidar_kf_track
+  * Change version 1.6.3 to 1.7.0 in pacakge.xml
+  * Modify CMake so that extrenal header would be loaded
+  * Remove obj_reproj from cv_tracker
+  * Add interface.yaml
+  * create common directory
+  * Add lidar_imm_ukf_pda_track
+  * create vision_detector and moved cv
+  * Modify interface.yaml and package.xml
+  * remove dpm_ocv
+  * moved directory
+  * Delete unnecessary launch file
+  * Delete rcnn related file and code
+  * separated dummy_track from cv_tracker
+  * separated klt_track from cv_tracker
+  * Fix a cmake
+  * Remove unnecessary dependency of lidar_euclidean_cluster_detect package
+  * Rename image_segmenter to vision_segment_enet_detect
+  * Remove unnecessary dependency of lidar_svm_detect package
+  * separated kf_track and fix a some compiling issue
+  * move viewers
+  * merge ndt_localizer and icp_localizer, and rename to lidar_localizer
+  * Remove unnecessary dependency of lidar_euclidean_track
+  * moved image lib
+  * add launch
+  * lib move under lidar_tracker
+  * Rename dpm_ttic to vision_dpm_ttic_detect
+  * rename yolo3detector to vision_yolo3_detect
+  * Modify cmake and package.xml in vision_dpm_ttic_detect
+  * moved sourcefiles into nodes dir
+  * moved sourcefiles into nodes dir
+  * Move cv_tracker/data folder and delete cv_tracker/model folder
+  * fix a package file and cmake
+  * Rename yolo2 -> vision_yolo2_detect
+  * fix a package file and cmake
+  * Fix package name of launch file
+  * Rename ssd to vision_ssd_detect
+  * fixed cmake and package for decerese dependencies
+  * remove top packages dir for detection
+  * fixed cmake for cuda
+  * Rename lane_detector to vision_lane_detect
+  * Modify package.xml in lidar-related packages
+  * Remove unnecessary dependencies in lidar_detector and lidar_tracker
+  * Modify computing.yaml for dpm_ttic
+  * Modify dpm_ttic launch file
+  * Remove/Add dependencies to trafficlight_recognizer
+  * Update data folder in dpm_ttic
+  * Modified CMake and package file in dpm_ttic.
+  * Remove src dir in imm_ukf_pda_track
+  * Fix bug for not starting run time manager
+  * Remove invalid dependency
+* Return disable_decision_maker to rosparam
+* Rename class and functions filter->replan
+* Fix message
+* Fix config message path
+* change can_translator
+  - Support to vehicle_status(can intermediate layer)
+  - Separate the can translator and the odometry.
+  - Support to output vehicle autonomous mode
+* add vehicle_status msg
+* Add end point offset option
+* Fix/cmake cleanup (`#1156 <https://github.com/CPFL/Autoware/issues/1156>`_)
+  * Initial Cleanup
+  * fixed also for indigo
+  * kf cjeck
+  * Fix road wizard
+  * Added travis ci
+  * Trigger CI
+  * Fixes to cv_tracker and lidar_tracker cmake
+  * Fix kitti player dependencies
+  * Removed unnecessary dependencies
+  * messages fixing for can
+  * Update build script travis
+  * Travis Path
+  * Travis Paths fix
+  * Travis test
+  * Eigen checks
+  * removed unnecessary dependencies
+  * Eigen Detection
+  * Job number reduced
+  * Eigen3 more fixes
+  * More Eigen3
+  * Even more Eigen
+  * find package cmake modules included
+  * More fixes to cmake modules
+  * Removed non ros dependency
+  * Enable industrial_ci for indidog and kinetic
+  * Wrong install command
+  * fix rviz_plugin install
+  * FastVirtualScan fix
+  * Fix Qt5 Fastvirtualscan
+  * Fixed qt5 system dependencies for rosdep
+  * NDT TKU Fix catkin not pacakged
+  * Fixes from industrial_ci
+* add ctrl_cmd/cmd/linear_acceletion
+* Correspond to new version of waypoint_csv(for decision_maker)
+* fix runtime_manager layout and description
+* Add config_callback for online waypoint tuning
+* Separate configration for speed planning against obstacle/stopline (Note: no logics changed)
+* parametrize detection area
+* add ratio for stopline target
+* Add a transition to stopstate to re-start only manually
+* add new param for decision_maker
+* Contributors: Abraham Monrroy, Akihito Ohsato, Dejan Pangercic, Kosuke Murakami, Yamato ANDO, Yuma, Yuma Nihei, Yusuke FUJII
+
+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
+* Added support to publish result of multiple traffic signals according to the lane
+  VectorMapServer Support to publish signals on current lane if current_pose and final_waypoints available
+* Initial modifications to feat_proj, tlr, context and vector_map loader, server and client to support different types of traffic signals
+* - Add new Node for object polygon representation and tracking (kf_contour_tracker)
+  - Add launch file and tune tracking parameters
+  - Test with Moriyama rosbag
+* Fixed:
+  - callback
+  - laneshift
+  Added:
+  - publisher for laneid
+  - new lanechange flag
+  - new param for decisionMaker
+* add to insert shift lane
+* Support to lanechange similar to state_machine(old) package
+* Changed path state recognition to the way based on /lane_waypoints_array
+* Fix build error, add msg definition
+* Rename and merge msgs
+* add path velocity smoothing
+* add msg of waypointstate for decision_maker
+* Feature/fusion_filter - fusion multiple lidar (`#842 <https://github.com/cpfl/autoware/issues/842>`_)
+  * Add fusion_filter to merge multiple lidar pointclouds
+  * Refactor fusion_filter
+  * Apply clang-format and rebase develop
+  * Add fusion_filter launch and runtime_manager config
+  * Fix names, fusion_filter -> points_concat_filter
+  * Fix build error in ros-indigo
+  * Fix some default message/frame names
+  * Refactor code and apply clang-format
+  * Add configrations for runtime_manager
+  * Fix CMake
+* refactor code
+* refactor code
+* refactor msg and add blinker to msg
+* Add ground_filter config for runtime_manager (`#828 <https://github.com/cpfl/autoware/issues/828>`_)
+* Ray Ground Filter Initial Commit
+* add approximate_ndt_mapping (`#811 <https://github.com/cpfl/autoware/issues/811>`_)
+* add new msg and rename msg
+* add mqtt sender
+* Contributors: AMC, Akihito Ohsato, Yamato ANDO, Yuki Iida, Yuki Kitsukawa, Yusuke FUJII, hatem-darweesh
+
+1.5.1 (2017-09-25)
+------------------
+* Release/1.5.1 (`#816 <https://github.com/cpfl/autoware/issues/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
+* update decision maker config
+* Add to support dynamical parameter for decision_maker
+* Add decision_maker config
+* add config parameter
+* autoware_msgs does not depend on jsk_rviz_plugin, cmake and package.xml were not correct
+* Contributors: Dejan Pangercic, Yusuke FUJII
+
+1.4.0 (2017-08-04)
+------------------
+* version number must equal current release number so we can start releasing in the future
+* added changelogs
+* Contributors: Dejan Pangercic
+
+1.3.1 (2017-07-16)
+------------------
+
+1.3.0 (2017-07-14)
+------------------
+* convert to autoware_msgs
+* Contributors: YamatoAndo
+
+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)
+------------------

src/ros/catkin/src/autoware_config_msgs/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_config_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+    ConfigApproximateNDTMapping.msg
+    ConfigCarDPM.msg
+    ConfigCarFusion.msg
+    ConfigCarKF.msg
+    ConfigCompareMapFilter.msg
+    ConfigDecisionMaker.msg
+    ConfigDistanceFilter.msg
+    ConfigICP.msg
+    ConfigLaneRule.msg
+    ConfigLaneSelect.msg
+    ConfigLaneStop.msg
+    ConfigLatticeVelocitySet.msg
+    ConfigNDT.msg
+    ConfigNDTMapping.msg
+    ConfigNDTMappingOutput.msg
+    ConfigPedestrianDPM.msg
+    ConfigPedestrianFusion.msg
+    ConfigPedestrianKF.msg
+    ConfigPlannerSelector.msg
+    ConfigPoints2Polygon.msg
+    ConfigRandomFilter.msg
+    ConfigRayGroundFilter.msg
+    ConfigRcnn.msg
+    ConfigRingFilter.msg
+    ConfigRingGroundFilter.msg
+    ConfigSSD.msg
+    ConfigTwistFilter.msg
+    ConfigVelocitySet.msg
+    ConfigVoxelGridFilter.msg
+    ConfigWaypointFollower.msg
+    ConfigWaypointReplanner.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+    std_msgs
+)
+
+catkin_package(CATKIN_DEPENDS
+  message_runtime
+  std_msgs
+)

src/ros/catkin/src/autoware_config_msgs/msg/ConfigApproximateNDTMapping.msg

@@ -0,0 +1,10 @@
+Header header
+float32 resolution
+float32 step_size
+float32 trans_epsilon
+int32 max_iterations
+float32 leaf_size
+float32 min_scan_range
+float32 max_scan_range
+float32 min_add_scan_shift
+float32 max_submap_size

src/ros/catkin/src/autoware_config_msgs/msg/ConfigCarDPM.msg

@@ -0,0 +1,6 @@
+Header header
+float32 score_threshold
+float32 group_threshold
+int32 Lambda
+int32 num_cells
+int32 num_bins

src/ros/catkin/src/autoware_config_msgs/msg/ConfigCarFusion.msg

@@ -0,0 +1,6 @@
+Header header
+float32 min_low_height
+float32 max_low_height
+float32 max_height
+int32 min_points
+float32 dispersion

src/ros/catkin/src/autoware_config_msgs/msg/ConfigCarKF.msg

@@ -0,0 +1,9 @@
+Header header
+int32 initial_lifespan
+int32 default_lifespan
+float32 noise_covariance
+float32 measurement_noise_covariance
+float32 error_estimate_covariance
+float32 percentage_of_overlapping
+int32 orb_features
+int32 use_orb

src/ros/catkin/src/autoware_config_msgs/msg/ConfigCompareMapFilter.msg

@@ -0,0 +1,3 @@
+float32 distance_threshold
+float32 min_clipping_height
+float32 max_clipping_height

src/ros/catkin/src/autoware_config_msgs/msg/ConfigDecisionMaker.msg

@@ -0,0 +1,14 @@
+Header header
+bool auto_mission_reload
+bool auto_engage
+bool auto_mission_change
+bool use_fms
+bool disuse_vector_map
+bool sim_mode
+bool insert_stop_line_wp
+uint32 num_of_steer_behind
+float64 change_threshold_dist
+float64 change_threshold_angle
+float64 goal_threshold_dist
+float64 goal_threshold_vel
+float64 stopped_vel

src/ros/catkin/src/autoware_config_msgs/msg/ConfigDistanceFilter.msg

@@ -0,0 +1,2 @@
+int32 sample_num
+float32 measurement_range

src/ros/catkin/src/autoware_config_msgs/msg/ConfigICP.msg

@@ -0,0 +1,15 @@
+Header header
+int32 init_pos_gnss
+float32 x
+float32 y
+float32 z
+float32 roll
+float32 pitch
+float32 yaw
+int32 use_predict_pose
+float32 error_threshold
+int32 maximum_iterations
+float32 transformation_epsilon
+float32 max_correspondence_distance
+float32 euclidean_fitness_epsilon
+float32 ransac_outlier_rejection_threshold

src/ros/catkin/src/autoware_config_msgs/msg/ConfigLaneRule.msg

@@ -0,0 +1,6 @@
+Header header
+float32 acceleration
+float32 stopline_search_radius
+int32 number_of_zeros_ahead
+int32 number_of_zeros_behind
+int32 number_of_smoothing_count

src/ros/catkin/src/autoware_config_msgs/msg/ConfigLaneSelect.msg

@@ -0,0 +1,6 @@
+Header header
+float32 distance_threshold_neighbor_lanes
+float32 lane_change_interval
+float32 lane_change_target_ratio
+float32 lane_change_target_minimum
+float32 vector_length_hermite_curve

src/ros/catkin/src/autoware_config_msgs/msg/ConfigLaneStop.msg

@@ -0,0 +1,2 @@
+Header header
+bool manual_detection

src/ros/catkin/src/autoware_config_msgs/msg/ConfigLatticeVelocitySet.msg

@@ -0,0 +1,10 @@
+Header header
+float32 others_distance
+float32 detection_range
+int32 threshold_points
+float32 detection_height_top
+float32 detection_height_bottom
+float32 deceleration
+float32 velocity_change_limit
+float32 deceleration_range
+float32 temporal_waypoints_size

src/ros/catkin/src/autoware_config_msgs/msg/ConfigNDT.msg

@@ -0,0 +1,23 @@
+Header header
+int32 init_pos_gnss
+float32 x
+float32 y
+float32 z
+float32 roll
+float32 pitch
+float32 yaw
+#int32 lidar_original
+#int32 max
+#int32 min
+#int32 layer
+int32 use_predict_pose
+float32 error_threshold
+float32 resolution
+float32 step_size
+float32 trans_epsilon
+int32 max_iterations
+#float32 leaf_size
+#float32 angle_error
+#float32 shift_x
+#float32 shift_y
+#float32 shift_z

src/ros/catkin/src/autoware_config_msgs/msg/ConfigNDTMapping.msg

@@ -0,0 +1,9 @@
+Header header
+float32 resolution
+float32 step_size
+float32 trans_epsilon
+int32 max_iterations
+float32 leaf_size
+float32 min_scan_range
+float32 max_scan_range
+float32 min_add_scan_shift

src/ros/catkin/src/autoware_config_msgs/msg/ConfigNDTMappingOutput.msg

@@ -0,0 +1,3 @@
+Header header
+string filename
+float32 filter_res

src/ros/catkin/src/autoware_config_msgs/msg/ConfigPedestrianDPM.msg

@@ -0,0 +1,6 @@
+Header header
+float32 score_threshold
+float32 group_threshold
+int32 Lambda
+int32 num_cells
+int32 num_bins

src/ros/catkin/src/autoware_config_msgs/msg/ConfigPedestrianFusion.msg

@@ -0,0 +1,6 @@
+Header header
+float32 min_low_height
+float32 max_low_height
+float32 max_height
+int32 min_points
+float32 dispersion

src/ros/catkin/src/autoware_config_msgs/msg/ConfigPedestrianKF.msg

@@ -0,0 +1,9 @@
+Header header
+int32 initial_lifespan
+int32 default_lifespan
+float32 noise_covariance
+float32 measurement_noise_covariance
+float32 error_estimate_covariance
+float32 percentage_of_overlapping
+int32 orb_features
+int32 use_orb

src/ros/catkin/src/autoware_config_msgs/msg/ConfigPlannerSelector.msg

@@ -0,0 +1,4 @@
+Header header
+int32 latency_num
+int32 waypoints_num
+float32 convergence_num

src/ros/catkin/src/autoware_config_msgs/msg/ConfigPoints2Polygon.msg

@@ -0,0 +1,9 @@
+Header header
+int32 k_search
+float32 search_radius
+float32 mu
+int32 maximum_nearest_neighbors
+float32 maximum_surface_angle
+float32 minimum_angle
+float32 maximum_angle
+bool normal_consistency

src/ros/catkin/src/autoware_config_msgs/msg/ConfigRandomFilter.msg

@@ -0,0 +1,2 @@
+int32 sample_num
+float32 measurement_range

src/ros/catkin/src/autoware_config_msgs/msg/ConfigRayGroundFilter.msg

@@ -0,0 +1,11 @@
+std_msgs/Header header
+
+float64  sensor_height
+float64  clipping_height
+float64  min_point_distance
+float64  radial_divider_angle
+float64  concentric_divider_distance
+float64  local_max_slope
+float64  general_max_slope
+float64  min_height_threshold
+float64  reclass_distance_threshold

src/ros/catkin/src/autoware_config_msgs/msg/ConfigRcnn.msg

@@ -0,0 +1,10 @@
+Header  header
+float32 score_threshold #minimum score required to keep the detection [0.0, 1.0] (default 0.6)
+float32 group_threshold #minimum overlap percentage area required to supress detections(NMS threshold) [0.0, 1.0] (default 0.3)
+float32 slices_overlap  #overlap percentage between image slices [0.0, 1.0] (default 0.7)
+float32 b_mean
+float32 g_mean
+float32 r_mean
+uint8   image_slices    #number of times to slice the image and search (1, 100], larger value might improve detection but reduce performance (default 16)
+uint8   use_gpu
+uint8   gpu_device_id

src/ros/catkin/src/autoware_config_msgs/msg/ConfigRingFilter.msg

@@ -0,0 +1,3 @@
+int32 ring_div
+float32 voxel_leaf_size
+float32 measurement_range

src/ros/catkin/src/autoware_config_msgs/msg/ConfigRingGroundFilter.msg

@@ -0,0 +1,4 @@
+string sensor_model
+float32 sensor_height
+float32 max_slope
+float32 vertical_thres

src/ros/catkin/src/autoware_config_msgs/msg/ConfigSSD.msg

@@ -0,0 +1,2 @@
+Header  header
+float32 score_threshold #minimum score required to keep the detection [0.0, 1.0] (default 0.6)

src/ros/catkin/src/autoware_config_msgs/msg/ConfigTwistFilter.msg

@@ -0,0 +1,7 @@
+Header header
+bool use_decision_maker
+float32 lateral_accel_limit
+float32 lateral_jerk_limit
+float32 lowpass_gain_linear_x
+float32 lowpass_gain_angular_z
+float32 lowpass_gain_steering_angle

src/ros/catkin/src/autoware_config_msgs/msg/ConfigVelocitySet.msg

@@ -0,0 +1,12 @@
+Header header
+float32 stop_distance_obstacle
+float32 stop_distance_stopline
+float32 detection_range
+int32 threshold_points
+float32 detection_height_top
+float32 detection_height_bottom
+float32 deceleration_obstacle
+float32 deceleration_stopline
+float32 velocity_change_limit
+float32 deceleration_range
+float32 temporal_waypoints_size

src/ros/catkin/src/autoware_config_msgs/msg/ConfigVoxelGridFilter.msg

@@ -0,0 +1,2 @@
+float32 voxel_leaf_size
+float32 measurement_range

src/ros/catkin/src/autoware_config_msgs/msg/ConfigWaypointFollower.msg

@@ -0,0 +1,8 @@
+Header header
+int32 param_flag
+float32 velocity
+float32 lookahead_distance
+float32 lookahead_ratio
+float32 minimum_lookahead_distance
+float32 displacement_threshold
+float32 relative_angle_threshold

src/ros/catkin/src/autoware_config_msgs/msg/ConfigWaypointReplanner.msg

@@ -0,0 +1,18 @@
+string multi_lane_csv
+bool replanning_mode
+bool use_decision_maker
+float32 velocity_max
+float32 velocity_min
+float32 accel_limit
+float32 decel_limit
+float32 radius_thresh
+float32 radius_min
+bool resample_mode
+float32 resample_interval
+int32 velocity_offset
+int32 end_point_offset
+int32 braking_distance
+bool replan_curve_mode
+bool replan_endpoint_mode
+bool overwrite_vmax_mode
+bool realtime_tuning_mode

src/ros/catkin/src/autoware_config_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autoware_config_msgs</name>
+  <version>1.13.0</version>
+  <description>The autoware_config_msgs package</description>
+  <maintainer email="yusuke.fujii@tier4.jp">Yusuke Fujii</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

src/ros/catkin/src/autoware_health_checker/.gitignore

@@ -0,0 +1,2 @@
+data/node_depends.dot
+data/node_depends.pdf

src/ros/catkin/src/autoware_health_checker/CHANGELOG.rst

@@ -0,0 +1,16 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_health_checker
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.11.0 (2019-03-21)
+-------------------
+* fix thread handling of node_status_publisher. (`#2058 <https://github.com/CPFL/Autoware/issues/2058>`_)
+* Revert "Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)" (`#2037 <https://github.com/CPFL/Autoware/issues/2037>`_)
+  This reverts commit e4187a7138eb90ad6f119eb35f824b16465aefda.
+  Reverts `#2012 <https://github.com/CPFL/Autoware/issues/2012>`_
+  Merged without adequate description of the bug or fixes made
+* Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)
+* Feature/autoware health analyzer (`#2004 <https://github.com/CPFL/Autoware/issues/2004>`_)
+* [Feature] enable display errors (`#1970 <https://github.com/CPFL/Autoware/issues/1970>`_)
+* Feature/autoware health checker (`#1943 <https://github.com/CPFL/Autoware/issues/1943>`_)
+* Contributors: Geoffrey Biggs, Masaya Kataoka, s-azumi

src/ros/catkin/src/autoware_health_checker/CMakeLists.txt

@@ -0,0 +1,105 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_health_checker)
+
+add_compile_options(-std=c++14)
+
+find_package(catkin REQUIRED COMPONENTS
+  autoware_system_msgs
+  roscpp
+  diagnostic_msgs
+  jsk_rviz_plugins
+  rosunit
+  rosgraph_msgs
+  ros_observer
+  roslint
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES health_checker system_status_subscriber
+  CATKIN_DEPENDS autoware_system_msgs rosgraph_msgs
+)
+
+set(ROSLINT_CPP_OPTS "--filter=-build/c++11")
+roslint_cpp()
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+set(HEALTH_CHECKER_SRC
+  src/health_checker/health_checker.cpp
+  src/health_checker/diag_buffer.cpp
+  src/health_checker/rate_checker.cpp
+  src/health_checker/value_manager.cpp
+  src/health_checker/param_manager.cpp
+)
+add_library(health_checker
+  ${HEALTH_CHECKER_SRC}
+)
+target_link_libraries(health_checker ${catkin_LIBRARIES})
+add_dependencies(health_checker ${catkin_EXPORTED_TARGETS})
+
+add_library(system_status_subscriber
+  src/system_status_subscriber/system_status_subscriber.cpp
+)
+target_link_libraries(system_status_subscriber ${catkin_LIBRARIES})
+add_dependencies(system_status_subscriber ${catkin_EXPORTED_TARGETS})
+
+add_executable(health_aggregator
+  src/health_aggregator/health_aggregator_node.cpp
+  src/health_aggregator/health_aggregator.cpp
+  src/health_aggregator/status_monitor.cpp
+)
+target_link_libraries(health_aggregator health_checker ${catkin_LIBRARIES})
+add_dependencies(health_aggregator ${catkin_EXPORTED_TARGETS})
+
+add_executable(health_analyzer
+  src/health_analyzer/health_analyzer_node.cpp
+  src/health_analyzer/health_analyzer.cpp
+)
+target_link_libraries(health_analyzer ${catkin_LIBRARIES})
+add_dependencies(health_analyzer ${catkin_EXPORTED_TARGETS})
+
+# CPP Execution programs
+set(CPP_EXEC_NAMES health_aggregator health_analyzer)
+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 health_checker system_status_subscriber
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Install launch
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+# Install config
+install(DIRECTORY config/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+)
+
+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
+  ${HEALTH_CHECKER_SRC})
+  target_link_libraries(test-autoware_health_checker
+  ${catkin_LIBRARIES})
+
+  roslint_add_test()
+endif ()

src/ros/catkin/src/autoware_health_checker/config/README.md

@@ -0,0 +1,58 @@
+The format of the threshold monitoring setting file is as follows.
+
+[1] You must list all monitored keys.
+If you want to know all the keys that can be set, run all the necessary nodes and run the following command.
+`rosparam dump dump.yaml /diag_reference`
+
+[2] You must set monitoring target threshold depends on your environment.
+Anything key not listed here will be excluded from monitoring.
+
+If you create it based on dump.yaml, don't forget to add the namespace health_checker at the top.
+
+```
+health_checker:
+  your_monitoring_target1:
+    min:
+      warn: double_warn_value_threshold
+      error: double_error_value_threshold
+      fatal: double_fatal_value_threshold
+    max:
+      warn: double_warn_value_threshold
+      error: double_error_value_threshold
+      fatal: double_fatal_value_threshold
+
+  your_monitoring_target2:
+    min:
+      # if you want to use default warn/error/fatal value,
+      # please remove warn/error/fatal.
+      error: double_error_value_threshold
+      fatal: double_fatal_value_threshold
+
+  your_monitoring_target3:
+    max: default
+      # if you want to use all default value in max/min/rate,
+      # please put "default" and remove warn/error/fatal.
+
+  your_monitoring_target4: default
+    # if you want to use all default value in the key,
+    # please put "default" and remove max/min/rate.
+    ...
+
+  topic_rate_topic1:
+    pub_node: /your_node_a
+    sub_node: /your_node_b
+    topic_name: /your_topic
+    rate:
+      warn: double_warning_rate_threshold
+      error: double_error_rate_threshold
+      fatal: double_fatal_rate_threshold
+
+  topic_rate_topic2:
+    pub_node: /your_node_a
+    sub_node: /your_node_b
+    topic_name: /your_topic
+    rate: default
+    ...
+```
+[3] You can load these threshold online.
+`rosparam load this_file`

src/ros/catkin/src/autoware_health_checker/data/generate_pdf.sh

@@ -0,0 +1 @@
+dot -T pdf node_depends.dot -o node_depends.pdf

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/constants.h

@@ -0,0 +1,58 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_CONSTANTS_H
+#define AUTOWARE_HEALTH_CHECKER_CONSTANTS_H
+
+#include <string>
+#include <autoware_system_msgs/DiagnosticStatus.h>
+
+namespace autoware_health_checker
+{
+/**
+ * \brief ErrorLevel means the seriousness of abnormalities.
+ * This value is managed by autoware_system_msgs::DiagnosticStatus.
+ */
+using ErrorLevel = uint8_t;
+
+/**
+ * \brief ErrorType means the type of abnormalities.
+ * This value is managed by autoware_system_msgs::DiagnosticStatus.
+ */
+using ErrorType = uint8_t;
+
+/**
+ * \brief ErrorKey uniquely identifies each self-monitoring target
+ * for parameter management and troubleshooting.
+ * e.g. "topic_rate_***_slow", "value_***_high", etc.
+ */
+using ErrorKey = std::string;
+
+/**
+ * \brief ThresholdType represents the threshold type
+ * of the self-monitoring target.
+ * e.g. "min", "max", and "range".
+ */
+using ThreshType = std::string;
+constexpr double BUFFER_DURATION = 0.5;
+constexpr double NODE_STATUS_UPDATE_RATE = 10.0;
+constexpr double SYSTEM_UPDATE_RATE = 30.0;
+}  // namespace autoware_health_checker
+
+#endif  // AUTOWARE_HEALTH_CHECKER_CONSTANTS_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_aggregator/health_aggregator.h

@@ -0,0 +1,92 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_HEALTH_AGGREGATOR_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_HEALTH_AGGREGATOR_H
+// headers in ROS
+#include <diagnostic_msgs/DiagnosticArray.h>
+#include <jsk_rviz_plugins/OverlayText.h>
+#include <ros/ros.h>
+
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+#include <autoware_health_checker/health_checker/param_manager.h>
+#include <autoware_health_checker/health_aggregator/status_monitor.h>
+#include <autoware_system_msgs/NodeStatus.h>
+#include <autoware_system_msgs/SystemStatus.h>
+
+// headers in boost
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <boost/property_tree/json_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+// headers in STL
+#include <map>
+#include <mutex>
+#include <string>
+#include <vector>
+
+class HealthAggregator
+{
+public:
+  using ErrorLevel = autoware_health_checker::ErrorLevel;
+  HealthAggregator(ros::NodeHandle nh, ros::NodeHandle pnh);
+  void run();
+
+private:
+  using AwHwStatus = autoware_system_msgs::HardwareStatus;
+  using AwHwStatusArray = std::vector<AwHwStatus>;
+  using AwNodeStatus = autoware_system_msgs::NodeStatus;
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  using AwDiagStatusArray = autoware_system_msgs::DiagnosticStatusArray;
+  using AwSysStatus = autoware_system_msgs::SystemStatus;
+  using RosDiagStatus = diagnostic_msgs::DiagnosticStatus;
+  using RosDiagArr = diagnostic_msgs::DiagnosticArray;
+  using ErrorKey = autoware_health_checker::ErrorKey;
+  const ErrorLevel convertHardwareLevel(const ErrorLevel& level) const;
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  ros::Publisher system_status_pub_;
+  std::map<ErrorLevel, ros::Publisher> text_pub_;
+  ros::Subscriber node_status_sub_;
+  ros::Subscriber diagnostic_array_sub_;
+  ros::Timer system_status_timer_, vital_timer_, ros_observer_timer_;
+  std::vector<std::string> detected_nodes_;
+  StatusMonitor status_monitor_;
+  void updateNodeStatus(const autoware_system_msgs::NodeStatus& node_status);
+  void publishSystemStatus(const ros::TimerEvent& event);
+  void nodeStatusCallback(const AwNodeStatus::ConstPtr& msg);
+  void diagnosticArrayCallback(const RosDiagArr::ConstPtr& msg);
+  std::string generateText(const std::vector<AwDiagStatus>& status);
+  jsk_rviz_plugins::OverlayText
+    generateOverlayText(const AwSysStatus& status, const ErrorLevel level);
+  std::vector<AwDiagStatus>
+    filterNodeStatus(const AwSysStatus& status, const ErrorLevel level);
+  boost::optional<AwHwStatusArray> convert(const RosDiagArr::ConstPtr& msg);
+  AwSysStatus system_status_;
+  autoware_health_checker::ParamManager param_manager_;
+  std::mutex mtx_;
+  void updateConnectionStatus(const ros::TimerEvent& event);
+  void rosObserverVitalCheck(const ros::TimerEvent& event);
+  double hardware_diag_rate_;
+  std::string hardware_diag_node_;
+};
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_HEALTH_AGGREGATOR_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_aggregator/status_monitor.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2015-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.
+ */
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_STATUS_MONITOR_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_STATUS_MONITOR_H
+
+#include <string>
+#include <map>
+#include <mutex>
+#include <ros/ros.h>
+#include <autoware_health_checker/constants.h>
+#include <autoware_system_msgs/NodeStatus.h>
+
+class TimeoutManager
+{
+public:
+  TimeoutManager() = default;
+  explicit TimeoutManager(const double time_out)
+    : timer_start_time_(ros::Time::now()), time_out_(time_out) {}
+  double getDuration(const ros::Time& current) const
+  {
+    return (current - timer_start_time_).toSec();
+  }
+  bool isOverLimit(const ros::Time& current) const
+  {
+    return (getDuration(current) > time_out_);
+  }
+  const ros::Time& getStartTime() const
+  {
+    return timer_start_time_;
+  }
+private:
+  ros::Time timer_start_time_;
+  double time_out_;
+};
+
+class StatusMonitor
+{
+public:
+  StatusMonitor();
+  void updateStamp(const std::string& name, const double timeout);
+  autoware_system_msgs::NodeStatus getMonitorStatus() const;
+private:
+  std::mutex mtx_;
+  std::map<std::string, TimeoutManager> timeout_manager_array_;
+};
+
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_AGGREGATOR_STATUS_MONITOR_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_analyzer/health_analyzer.h

@@ -0,0 +1,102 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_ANALYZER_HEALTH_ANALYZER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_ANALYZER_HEALTH_ANALYZER_H
+// headers in ROS
+#include <ros/package.h>
+#include <ros/ros.h>
+
+// headers in STL
+#include <map>
+#include <string>
+#include <vector>
+
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+#include <autoware_system_msgs/SystemStatus.h>
+
+// headers in boost
+#include <boost/optional.hpp>
+#include <boost/foreach.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/graph_utility.hpp>
+#include <boost/graph/graphviz.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+
+struct topic_property
+{
+  std::string node_pub;
+  std::string node_sub;
+};
+
+struct node_property
+{
+  std::string node_name;
+};
+
+
+class HealthAnalyzer
+{
+public:
+  HealthAnalyzer(ros::NodeHandle nh, ros::NodeHandle pnh);
+  ~HealthAnalyzer();
+
+private:
+  using graph_t = boost::adjacency_list<boost::listS, boost::vecS,
+  boost::bidirectionalS, node_property, topic_property>;
+  using vertex_t = graph_t::vertex_descriptor;
+  using edge_t = graph_t::edge_descriptor;
+  using adjacency_iterator_t = boost::graph_traits<graph_t>::adjacency_iterator;
+  using out_edge_iterator_t = boost::graph_traits<graph_t>::out_edge_iterator;
+
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  ros::Subscriber system_status_sub_;
+  ros::Subscriber topic_statistics_sub_;
+  ros::Publisher system_status_summary_pub_;
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  void systemStatusCallback(
+    const autoware_system_msgs::SystemStatus::ConstPtr& msg);
+  void generateDependGraph(const autoware_system_msgs::SystemStatus& status);
+  void addDepend(const rosgraph_msgs::TopicStatistics& statistics);
+  autoware_system_msgs::SystemStatus filterSystemStatus(
+    const autoware_system_msgs::SystemStatus& status);
+  std::vector<std::string> findWarningNodes(
+    const autoware_system_msgs::SystemStatus& status);
+  std::vector<std::string> findErrorNodes(
+    const autoware_system_msgs::SystemStatus& status);
+  std::vector<std::string> findRootNodes(
+    const std::vector<std::string>& target_nodes);
+  boost::optional<vertex_t> getTargetNode(const std::string& target_node);
+  int countWarn(const autoware_system_msgs::SystemStatus& msg);
+  void writeDot();
+  graph_t depend_graph_;
+  int warn_nodes_count_threshold_;
+  template <typename T> bool
+    isAlreadyExist(const std::vector<T>& vector, const T& target) const
+  {
+    auto result = std::find(vector.begin(), vector.end(), target);
+    return (result != vector.end());
+  }
+};
+
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_ANALYZER_HEALTH_ANALYZER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_checker/diag_buffer.h

@@ -0,0 +1,63 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_DIAG_BUFFER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_DIAG_BUFFER_H
+// headers in Autoare
+#include <autoware_health_checker/constants.h>
+#include <autoware_system_msgs/DiagnosticStatusArray.h>
+
+// headers in STL
+#include <map>
+#include <mutex>
+#include <string>
+#include <vector>
+
+// headers in ROS
+#include <ros/ros.h>
+
+namespace autoware_health_checker
+{
+class DiagBuffer
+{
+public:
+  DiagBuffer(ErrorKey key, ErrorType type, std::string description,
+             double buffer_duration);
+  void addDiag(autoware_system_msgs::DiagnosticStatus status);
+  autoware_system_msgs::DiagnosticStatusArray getAndClearData();
+  const ErrorType type;
+  const std::string description;
+
+private:
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  std::mutex mtx_;
+  ErrorLevel getErrorLevel();
+  void updateBuffer();
+  ErrorKey key_;
+  ros::Duration buffer_duration_;
+  std::map<ErrorLevel, autoware_system_msgs::DiagnosticStatusArray> buffer_;
+  autoware_system_msgs::DiagnosticStatusArray filterBuffer(
+    ros::Time now, ErrorLevel level);
+  ros::Publisher status_pub_;
+  bool isOlderTimestamp(const autoware_system_msgs::DiagnosticStatus &a,
+                        const autoware_system_msgs::DiagnosticStatus &b);
+};
+}  // namespace autoware_health_checker
+
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_DIAG_BUFFER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_checker/health_checker.h

@@ -0,0 +1,147 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_HEALTH_CHECKER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_HEALTH_CHECKER_H
+// headers in ROS
+#include <ros/ros.h>
+
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+#include <autoware_health_checker/health_checker/diag_buffer.h>
+#include <autoware_health_checker/health_checker/rate_checker.h>
+#include <autoware_health_checker/health_checker/value_manager.h>
+#include <autoware_system_msgs/NodeStatus.h>
+
+// headers in STL
+#include <functional>
+#include <map>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <utility>
+#include <thread>
+#include <atomic>
+#include <chrono>
+
+// headers in boost
+#include <boost/bind.hpp>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <boost/property_tree/json_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+namespace autoware_health_checker
+{
+using MinMax = std::pair<double, double>;
+class HealthChecker
+{
+public:
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  using AwDiagStatusArray = autoware_system_msgs::DiagnosticStatusArray;
+  HealthChecker(ros::NodeHandle nh, ros::NodeHandle pnh);
+  ~HealthChecker();
+  void ENABLE();
+  ErrorLevel CHECK_MIN_VALUE(const ErrorKey& key, const double value,
+    const double warn_value, const double error_value,
+    const double fatal_value, const std::string& description);
+  ErrorLevel CHECK_MAX_VALUE(const ErrorKey& key, const double value,
+    const double warn_value, const double error_value,
+    const double fatal_value, const std::string& description);
+  ErrorLevel CHECK_RANGE(const ErrorKey& key, const double value,
+    const MinMax warn_value, const MinMax error_value,
+    const MinMax fatal_value, const std::string& description);
+  template <class T> ErrorLevel CHECK_VALUE(
+    const ErrorKey& key, const T& value,
+    std::function<ErrorLevel(T value)> check_func,
+    std::function<boost::property_tree::ptree(T value)> value_json_func,
+    const std::string& description)
+  {
+    value_manager_.addCandidate(key);
+    if (value_manager_.isNotFound(key))
+    {
+      return AwDiagStatus::UNDEFINED;
+    }
+    ErrorLevel check_result = check_func(value);
+    boost::property_tree::ptree pt = value_json_func(value);
+    std::stringstream ss;
+    write_json(ss, pt);
+    AwDiagStatus new_status;
+    new_status.key = key;
+    new_status.level = check_result;
+    new_status.description = description;
+    new_status.value = ss.str();
+    new_status.header.stamp = ros::Time::now();
+    new_status.type = AwDiagStatus::INVALID_VALUE;
+    return SET_DIAG_STATUS(new_status);
+  }
+  void CHECK_RATE(const ErrorKey& key, const double warn_rate,
+    const double error_rate, const double fatal_rate,
+    const std::string& description);
+  ErrorLevel CHECK_TRUE(const ErrorKey& key, const bool value,
+    const ErrorLevel level, const std::string& description);
+  ErrorLevel SET_DIAG_STATUS(
+    const autoware_system_msgs::DiagnosticStatus& status);
+  void NODE_ACTIVATE()
+  {
+    std::lock_guard<std::mutex> lock(mtx_);
+    node_activated_ = true;
+  };
+  void NODE_DEACTIVATE()
+  {
+    std::lock_guard<std::mutex> lock(mtx_);
+    node_activated_ = false;
+  };
+  bool getNodeStatus()
+  {
+    return node_activated_;
+  };
+
+private:
+  std::vector<ErrorKey> getKeys();
+  std::vector<ErrorKey> getRateCheckerKeys();
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  std::thread node_status_publish_thread_;
+  ValueManager value_manager_;
+  std::map<ErrorKey, std::unique_ptr<DiagBuffer>> diag_buffers_;
+  std::map<ErrorKey, std::unique_ptr<RateChecker>> rate_checkers_;
+  ros::Publisher status_pub_;
+  bool keyExist(const ErrorKey& key) const;
+  bool addNewBuffer(const ErrorKey& key, const ErrorType type,
+    const std::string& description);
+  template <typename T> autoware_system_msgs::DiagnosticStatus
+    setValueCommon(
+      const ErrorKey& key, const T& value, const std::string& desc);
+  template <typename T> std::string valueToJson(const T& value)
+  {
+    std::stringstream ss;
+    boost::property_tree::ptree pt;
+    pt.put("value", value);
+    write_json(ss, pt);
+    return ss.str();
+  }
+  void publishStatus();
+  bool node_activated_;
+  std::atomic<bool> is_shutdown_;
+  std::mutex mtx_;
+};
+}  // namespace autoware_health_checker
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_HEALTH_CHECKER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_checker/param_manager.h

@@ -0,0 +1,46 @@
+/*
+ * Copyright 2015-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.
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_PARAM_MANAGER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_PARAM_MANAGER_H
+
+#include <string>
+#include <set>
+#include <mutex>
+#include <ros/ros.h>
+#include <autoware_health_checker/constants.h>
+
+namespace autoware_health_checker
+{
+class ParamManager
+{
+public:
+  ParamManager(ros::NodeHandle nh, ros::NodeHandle pnh);
+  bool isNotFound(const ErrorKey& key);
+  bool isNotFound(const ErrorKey& ns, const ErrorKey& key);
+  void addCandidate(const ErrorKey& key);
+  const XmlRpc::XmlRpcValue& getParams();
+private:
+  void getParams(const ros::TimerEvent& event);
+  void setParams(const ros::TimerEvent& event);
+  ros::NodeHandle nh_, pnh_;
+  ros::Timer get_timer_, set_timer_;
+  std::set<ErrorKey> cached_candidate_key_;
+  XmlRpc::XmlRpcValue params_;
+  std::mutex param_mtx_, key_mtx_;
+};
+}  // namespace autoware_health_checker
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_PARAM_MANAGER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_checker/rate_checker.h

@@ -0,0 +1,65 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_RATE_CHECKER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_RATE_CHECKER_H
+// headers in ROS
+#include <ros/ros.h>
+
+// headers in STL
+#include <mutex>
+#include <vector>
+#include <string>
+#include <utility>
+
+// headers in Boost
+#include <boost/optional.hpp>
+
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+
+namespace autoware_health_checker
+{
+using LevelRatePair = std::pair<ErrorLevel, double>;
+
+class RateChecker
+{
+public:
+  RateChecker(double buffer_duration, double warn_rate, double error_rate,
+              double fatal_rate, std::string description);
+  void check();
+  boost::optional<LevelRatePair> getErrorLevelAndRate();
+  boost::optional<ErrorLevel> getErrorLevel();
+  boost::optional<double> getRate();
+  void setRate(double warn_rate, double error_rate, double fatal_rate);
+  const std::string description;
+
+private:
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  ros::Time start_time_;
+  void update();
+  std::vector<ros::Time> data_;
+  double buffer_duration_;
+  double warn_rate_;
+  double error_rate_;
+  double fatal_rate_;
+  std::mutex mtx_;
+};
+}  // namespace autoware_health_checker
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_RATE_CHECKER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/health_checker/value_manager.h

@@ -0,0 +1,59 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_VALUE_MANAGER_H
+#define AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_VALUE_MANAGER_H
+
+// headers in ROS
+#include <ros/ros.h>
+
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+#include <autoware_health_checker/health_checker/param_manager.h>
+
+// headers in STL
+#include <map>
+#include <mutex>
+#include <string>
+#include <utility>
+
+// headers in Boost
+#include <boost/optional.hpp>
+namespace autoware_health_checker
+{
+class ValueManager : public ParamManager
+{
+public:
+  using ErrorCategory = std::pair<ErrorKey, ThreshType>;
+  using ErrorSummary = std::pair<ErrorCategory, ErrorLevel>;
+
+  ValueManager(ros::NodeHandle nh, ros::NodeHandle pnh);
+  void setDefaultValue(
+    const ErrorKey& key, const ThreshType& thresh_type, const double warn_value,
+    const double error_value, const double fatal_value);
+  boost::optional<double> getValue(const ErrorKey& key,
+    const ThreshType& thresh_type, const ErrorLevel level);
+  bool isEmpty(const ErrorKey& key) const;
+
+private:
+  using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+  std::map<ErrorSummary, double> error_details_;
+};
+}  // namespace autoware_health_checker
+#endif  // AUTOWARE_HEALTH_CHECKER_HEALTH_CHECKER_VALUE_MANAGER_H

src/ros/catkin/src/autoware_health_checker/include/autoware_health_checker/system_status_subscriber/system_status_subscriber.h

@@ -0,0 +1,54 @@
+/*
+ * 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
+ */
+
+#ifndef AUTOWARE_HEALTH_CHECKER_SYSTEM_STATUS_SUBSCRIBER_SYSTEM_STATUS_SUBSCRIBER_H
+#define AUTOWARE_HEALTH_CHECKER_SYSTEM_STATUS_SUBSCRIBER_SYSTEM_STATUS_SUBSCRIBER_H
+// headers in Autoware
+#include <autoware_health_checker/constants.h>
+#include <autoware_system_msgs/SystemStatus.h>
+
+// headers in ROS
+#include <ros/ros.h>
+
+// headers in STL
+#include <functional>
+#include <mutex>
+#include <vector>
+
+namespace autoware_health_checker
+{
+class SystemStatusSubscriber
+{
+public:
+  SystemStatusSubscriber(ros::NodeHandle nh, ros::NodeHandle pnh);
+  void enable();
+  void
+  addCallback(std::function<void(std::shared_ptr<autoware_system_msgs::SystemStatus>)> func);
+
+private:
+  void
+  systemStatusCallback(const autoware_system_msgs::SystemStatus::ConstPtr msg);
+  ros::Subscriber status_sub_;
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  std::vector<std::function<void(std::shared_ptr<autoware_system_msgs::SystemStatus>)>> functions_;
+};
+}  // namespace autoware_health_checker
+
+#endif  // AUTOWARE_HEALTH_CHECKER_SYSTEM_STATUS_SUBSCRIBER_SYSTEM_STATUS_SUBSCRIBER_H

src/ros/catkin/src/autoware_health_checker/launch/health_analyzer.launch

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="output" default="log"/>
+  <arg name="warn_nodes_count_threshold" default="30"/>
+  <node pkg="autoware_health_checker" type="health_analyzer" name="health_analyzer" output="$(arg output)" respawn="true" respawn_delay="0">
+    <param name="warn_nodes_count_threshold" value="$(arg warn_nodes_count_threshold)"/>
+  </node>
+</launch>

src/ros/catkin/src/autoware_health_checker/launch/health_checker.launch

@@ -0,0 +1,4 @@
+<launch>
+  <arg name="output" default="log"/>
+  <node pkg="autoware_health_checker" type="health_aggregator" name="health_aggregator" output="$(arg output)" respawn="true" respawn_delay="0" />
+</launch>

src/ros/catkin/src/autoware_health_checker/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autoware_health_checker</name>
+  <version>1.12.0</version>
+  <description>The autoware_health_checker package</description>
+  <maintainer email="masaya.kataoka@tier4.jp">MasayaKataoka</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <test_depend>rostest</test_depend>
+
+  <depend>autoware_system_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>jsk_rviz_plugins</depend>
+  <depend>roscpp</depend>
+  <depend>rosgraph_msgs</depend>
+  <depend>rosunit</depend>
+  <depend>ros_observer</depend>
+  <depend>roslint</depend>
+</package>

src/ros/catkin/src/autoware_health_checker/src/health_aggregator/health_aggregator.cpp

@@ -0,0 +1,327 @@
+/*
+ * 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 <string>
+#include <vector>
+#include <regex>
+#include <autoware_health_checker/health_aggregator/health_aggregator.h>
+#include <ros_observer/lib_ros_observer.h>
+
+namespace
+{
+std::string changeToKeyFormat(const std::string& node_name)
+{
+  std::string changed_name(node_name);
+  if (changed_name.at(0) == '/')
+  {
+    changed_name.erase(changed_name.begin());
+  }
+  changed_name = std::regex_replace(changed_name, std::regex("/"), "_");
+  return changed_name;
+}
+
+
+boost::optional <std::string> getValidName(const std::string& orig)
+{
+  std::string changed(orig);
+  static const std::vector<std::string> delete_strings =
+  {
+    ".*:", ".*/", R"(\(.*\))",
+    R"([\[\]"\\(){}?.*+^$|!#%&'-=~`@;:,<> ])"
+  };
+  for (const auto& str : delete_strings)
+  {
+    changed = std::regex_replace(changed, std::regex(str), "");
+  }
+  std::string error;
+  return ros::names::validate(changed, error) ?
+    boost::optional<std::string>(changed) : boost::none;
+}
+}  // namespace
+
+HealthAggregator::HealthAggregator(ros::NodeHandle nh, ros::NodeHandle pnh)
+  : nh_(nh), pnh_(pnh), param_manager_(nh_, pnh)
+{
+  nh_.param("hardware_diag_node",
+    hardware_diag_node_, std::string("diagnostic_aggregator"));
+  nh_.param(hardware_diag_node_ + "/pub_rate", hardware_diag_rate_, 1.0);
+}
+
+void HealthAggregator::run()
+{
+  system_status_pub_ = nh_.advertise<AwSysStatus>("system_status", 10);
+  auto registerTextPublisher = [this](ErrorLevel level, std::string topic)
+  {
+    text_pub_[level] = pnh_.advertise<jsk_rviz_plugins::OverlayText>(topic, 1);
+    return;
+  };
+  registerTextPublisher(AwDiagStatus::OK, "ok_text");
+  registerTextPublisher(AwDiagStatus::WARN, "warn_text");
+  registerTextPublisher(AwDiagStatus::ERROR, "error_text");
+  registerTextPublisher(AwDiagStatus::FATAL, "fatal_text");
+  node_status_sub_ = nh_.subscribe("node_status", 10,
+    &HealthAggregator::nodeStatusCallback, this);
+  // ros::master::getNodes will continue to wait for a response from the master
+  // if the master goes down unless a timeout is specified.
+  // To avoid this, it is necessary to set a timeout.
+  ros::master::setRetryTimeout(ros::WallDuration(0.01));
+  diagnostic_array_sub_ = nh_.subscribe(
+    "diagnostics_agg", 10, &HealthAggregator::diagnosticArrayCallback, this);
+
+  ros::Duration duration(1.0 / autoware_health_checker::SYSTEM_UPDATE_RATE);
+  system_status_timer_ =
+    nh_.createTimer(duration, &HealthAggregator::publishSystemStatus, this);
+  vital_timer_ =
+    nh_.createTimer(duration, &HealthAggregator::updateConnectionStatus, this);
+  ros_observer_timer_ =
+    nh_.createTimer(duration, &HealthAggregator::rosObserverVitalCheck, this);
+}
+
+void HealthAggregator::updateNodeStatus(
+  const autoware_system_msgs::NodeStatus& node_status)
+{
+  auto& node_status_array = system_status_.node_status;
+  auto identify = [&node_status](const autoware_system_msgs::NodeStatus& status)
+  {
+    return status.node_name == node_status.node_name;
+  };
+  auto result =
+    std::find_if(node_status_array.begin(), node_status_array.end(), identify);
+  if (result != node_status_array.end())
+  {
+    *result = node_status;
+  }
+  else
+  {
+    node_status_array.emplace_back(node_status);
+  }
+}
+void HealthAggregator::publishSystemStatus(const ros::TimerEvent& event)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  system_status_.header.stamp = ros::Time::now();
+  updateNodeStatus(status_monitor_.getMonitorStatus());
+  system_status_.available_nodes = detected_nodes_;
+  system_status_pub_.publish(system_status_);
+  static const std::array<ErrorLevel, 4> level_array =
+  {
+    AwDiagStatus::OK,
+    AwDiagStatus::WARN,
+    AwDiagStatus::ERROR,
+    AwDiagStatus::FATAL
+  };
+  for (const auto& level : level_array)
+  {
+    text_pub_[level].publish(generateOverlayText(system_status_, level));
+  }
+}
+
+void HealthAggregator::rosObserverVitalCheck(const ros::TimerEvent& event)
+{
+  static constexpr double loop_rate = autoware_health_checker::SYSTEM_UPDATE_RATE;
+  static ShmVitalMonitor shm_ROvmon("RosObserver", loop_rate, VitalMonitorMode::CNT_MON);
+  static ShmVitalMonitor shm_HAvmon("HealthAggregator", loop_rate);
+
+  shm_ROvmon.run();
+  shm_HAvmon.run();
+}
+
+void HealthAggregator::updateConnectionStatus(const ros::TimerEvent& event)
+{
+  std::vector<std::string> detected_nodes;
+  ros::master::getNodes(detected_nodes);
+  detected_nodes_ = detected_nodes;
+}
+
+void HealthAggregator::nodeStatusCallback(const AwNodeStatus::ConstPtr& msg)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  updateNodeStatus(*msg);
+  static const double timeout =
+    1.0 / autoware_health_checker::NODE_STATUS_UPDATE_RATE * 2.0;
+  status_monitor_.updateStamp(changeToKeyFormat(msg->node_name), timeout);
+}
+
+void HealthAggregator::diagnosticArrayCallback(const RosDiagArr::ConstPtr& msg)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  auto status = convert(msg);
+  if (status)
+  {
+    system_status_.hardware_status = status.get();
+  }
+  static const double timeout = 1.0 / hardware_diag_rate_ * 2.0;
+  status_monitor_.updateStamp(changeToKeyFormat(hardware_diag_node_), timeout);
+}
+
+std::string HealthAggregator::generateText(
+  const std::vector<AwDiagStatus>& status)
+{
+  std::string text;
+  for (const auto& s : status)
+  {
+    text = text + s.description + "\n";
+  }
+  return text;
+}
+
+jsk_rviz_plugins::OverlayText
+HealthAggregator::generateOverlayText(const AwSysStatus& status,
+  const HealthAggregator::ErrorLevel level)
+{
+  jsk_rviz_plugins::OverlayText text;
+  text.action = text.ADD;
+  text.width = 640;
+  text.height = 640;
+  text.top = 0;
+  text.bg_color.r = 0;
+  text.bg_color.g = 0;
+  text.bg_color.b = 0;
+  text.bg_color.a = 0.7;
+  text.text_size = 20.0;
+  if (level == AwDiagStatus::OK)
+  {
+    text.left = 0;
+    text.fg_color.r = 0.0;
+    text.fg_color.g = 0.0;
+    text.fg_color.b = 1.0;
+    text.fg_color.a = 1.0;
+    text.text = generateText(filterNodeStatus(status, level));
+  }
+  else if (level == AwDiagStatus::WARN)
+  {
+    text.left = 640 * 1;
+    text.fg_color.r = 1.0;
+    text.fg_color.g = 1.0;
+    text.fg_color.b = 0.0;
+    text.fg_color.a = 1.0;
+    text.text = generateText(filterNodeStatus(status, level));
+  }
+  else if (level == AwDiagStatus::ERROR)
+  {
+    text.left = 640 * 2;
+    text.fg_color.r = 1.0;
+    text.fg_color.g = 0.0;
+    text.fg_color.b = 0.0;
+    text.fg_color.a = 1.0;
+    text.text = generateText(filterNodeStatus(status, level));
+  }
+  else if (level == AwDiagStatus::FATAL)
+  {
+    text.left = 640 * 3;
+    text.fg_color.r = 1.0;
+    text.fg_color.g = 1.0;
+    text.fg_color.b = 1.0;
+    text.fg_color.a = 1.0;
+    text.text = generateText(filterNodeStatus(status, level));
+  }
+  return text;
+}
+
+std::vector<HealthAggregator::AwDiagStatus>
+HealthAggregator::filterNodeStatus(const AwSysStatus& status,
+  const HealthAggregator::ErrorLevel level)
+{
+  std::vector<AwDiagStatus> ret;
+  for (const auto& node_status : status.node_status)
+  {
+    if (!node_status.node_activated)
+    {
+      continue;
+    }
+    for (const auto& node_status_array : node_status.status)
+    {
+      for (const auto& diag_status : node_status_array.status)
+      {
+        if (diag_status.level == level)
+        {
+          ret.emplace_back(diag_status);
+        }
+      }
+    }
+  }
+  return ret;
+}
+
+const HealthAggregator::ErrorLevel
+  HealthAggregator::convertHardwareLevel(const ErrorLevel& level) const
+{
+  return
+    (level == RosDiagStatus::OK) ? AwDiagStatus::OK :
+    (level == RosDiagStatus::WARN) ? AwDiagStatus::WARN :
+    (level == RosDiagStatus::ERROR) ? AwDiagStatus::ERROR :
+    (level == RosDiagStatus::STALE) ? AwDiagStatus::FATAL :
+    AwDiagStatus::UNDEFINED;
+}
+
+boost::optional<HealthAggregator::AwHwStatusArray>
+  HealthAggregator::convert(const RosDiagArr::ConstPtr& msg)
+{
+  AwHwStatusArray status_array;
+  if (msg->status.empty())
+  {
+    return boost::none;
+  }
+
+  for (const auto& hw_status : msg->status)
+  {
+    const auto ns = getValidName(hw_status.name);
+    if (!ns)
+    {
+      continue;
+    }
+    AwHwStatus status;
+    status.header = msg->header;
+    status.hardware_name = ns.get();
+    const ErrorLevel level = convertHardwareLevel(hw_status.level);
+    AwDiagStatusArray diag_array;
+    for (const auto& diag_status : hw_status.values)
+    {
+      const auto local_key = getValidName(diag_status.key);
+      if (!local_key)
+      {
+        continue;
+      }
+      const ErrorKey global_key = ns.get() + "/" + local_key.get();
+      param_manager_.addCandidate(global_key);
+      if (param_manager_.isNotFound(ns.get(), local_key.get()))
+      {
+        continue;
+      }
+      AwDiagStatus diag;
+      diag.header = msg->header;
+      diag.key = global_key;
+      diag.description = global_key;
+      diag.type = AwDiagStatus::HARDWARE;
+      diag.level = level;
+      std::stringstream ss;
+      boost::property_tree::ptree pt;
+      pt.put("value", diag_status.value);
+      write_json(ss, pt);
+      diag.value = ss.str();
+      diag_array.status.emplace_back(diag);
+    }
+    if (!diag_array.status.empty())
+    {
+      status.status.emplace_back(diag_array);
+      status_array.emplace_back(status);
+    }
+  }
+  return status_array;
+}

src/ros/catkin/src/autoware_health_checker/src/health_aggregator/health_aggregator_node.cpp

@@ -0,0 +1,33 @@
+/*
+ * 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 <ros/ros.h>
+
+#include <autoware_health_checker/health_aggregator/health_aggregator.h>
+
+int main(int argc, char *argv[])
+{
+  ros::init(argc, argv, "health_aggregator");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+  HealthAggregator agg(nh, pnh);
+  agg.run();
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/autoware_health_checker/src/health_aggregator/status_monitor.cpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright 2015-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.
+ */
+#include <string>
+#include <autoware_health_checker/health_aggregator/status_monitor.h>
+
+StatusMonitor::StatusMonitor() {}
+
+void StatusMonitor::updateStamp(const std::string& name, const double timeout)
+{
+  if (name.empty())
+  {
+    return;
+  }
+  std::lock_guard<std::mutex> lock(mtx_);
+  timeout_manager_array_[name] = TimeoutManager(timeout);
+}
+
+autoware_system_msgs::NodeStatus StatusMonitor::getMonitorStatus() const
+{
+  using DiagStatus = autoware_system_msgs::DiagnosticStatus;
+  using DiagStatusArray = autoware_system_msgs::DiagnosticStatusArray;
+  static const std::string my_name = ros::this_node::getName();
+  const ros::Time current = ros::Time::now();
+  autoware_system_msgs::NodeStatus my_status;
+  DiagStatus status_template;
+  status_template.key = "node_status_rate_slow";
+  status_template.description = "node_status rate slow";
+  status_template.type = DiagStatus::UNEXPECTED_RATE;
+  status_template.level = DiagStatus::OK;
+  for (const auto& timeout_manager : timeout_manager_array_)
+  {
+    DiagStatusArray status_array;
+    DiagStatus status(status_template);
+    const auto& node_name = timeout_manager.first;
+    const auto& node_timer = timeout_manager.second;
+    status.key = node_name + "_" + status.key;
+    status.description = node_name + " " + status.description;
+    std::stringstream ss;
+    ss << node_timer.getDuration(current);
+    status.value = ss.str();
+    if (node_timer.isOverLimit(current))
+    {
+      status.level = DiagStatus::ERROR;
+    }
+    status.header.stamp = node_timer.getStartTime();
+    status_array.status.emplace_back(status);
+    my_status.status.emplace_back(status_array);
+  }
+  my_status.header.stamp = ros::Time::now();
+  my_status.node_name = my_name;
+  my_status.node_activated = true;
+  return my_status;
+}

src/ros/catkin/src/autoware_health_checker/src/health_analyzer/health_analyzer.cpp

@@ -0,0 +1,254 @@
+/*
+ * 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 <string>
+#include <vector>
+#include <autoware_health_checker/health_analyzer/health_analyzer.h>
+
+HealthAnalyzer::HealthAnalyzer(ros::NodeHandle nh, ros::NodeHandle pnh)
+  : nh_(nh), pnh_(pnh)
+{
+  using SystemStatus = autoware_system_msgs::SystemStatus;
+  pnh_.param("warn_nodes_count_threshold", warn_nodes_count_threshold_, 30);
+  system_status_summary_pub_ =
+    nh_.advertise<SystemStatus>("system_status/summary", 1);
+  system_status_sub_ = nh_.subscribe(
+    "system_status", 1, &HealthAnalyzer::systemStatusCallback, this);
+}
+
+HealthAnalyzer::~HealthAnalyzer()
+{
+  writeDot();
+}
+
+int HealthAnalyzer::countWarn(const autoware_system_msgs::SystemStatus& msg)
+{
+  int count = 0;
+  for (const auto& status_array_list : msg.node_status)
+  {
+    for (const auto& status_array : status_array_list.status)
+    {
+      for (const auto& status : status_array.status)
+      {
+        if (status.level == AwDiagStatus::WARN)
+        {
+          count++;
+        }
+      }
+    }
+  }
+  for (const auto& status_array_list : msg.hardware_status)
+  {
+    for (const auto& status_array : status_array_list.status)
+    {
+      for (const auto& status : status_array.status)
+      {
+        if (status.level == AwDiagStatus::WARN)
+        {
+          count++;
+        }
+      }
+    }
+  }
+  return count;
+}
+
+std::vector<std::string> HealthAnalyzer::findWarningNodes(
+  const autoware_system_msgs::SystemStatus& sys_status)
+{
+  std::vector<std::string> ret;
+  auto isOverWarn = [](autoware_health_checker::ErrorLevel level)
+  {
+    return (
+      level == AwDiagStatus::WARN ||
+      level == AwDiagStatus::ERROR ||
+      level == AwDiagStatus::FATAL);
+  };
+  for (const auto& node_status : sys_status.node_status)
+  {
+    for (const auto& status_array : node_status.status)
+    {
+      for (const auto& status : status_array.status)
+      {
+        if (isAlreadyExist(ret, node_status.node_name))
+        {
+          continue;
+        }
+        else if (isOverWarn(status.level))
+        {
+          ret.emplace_back(node_status.node_name);
+        }
+      }
+    }
+  }
+  return ret;
+}
+
+std::vector<std::string> HealthAnalyzer::findErrorNodes(
+  const autoware_system_msgs::SystemStatus& sys_status)
+{
+  std::vector<std::string> ret;
+  auto isOverError = [](autoware_health_checker::ErrorLevel level)
+  {
+    return (
+      level == AwDiagStatus::ERROR ||
+      level == AwDiagStatus::FATAL);
+  };
+  for (const auto& node_status : sys_status.node_status)
+  {
+    for (const auto& status_array : node_status.status)
+    {
+      const auto& node_name = node_status.node_name;
+      for (const auto& status : status_array.status)
+      {
+        if (isAlreadyExist(ret, node_name) || !isOverError(status.level))
+        {
+          continue;
+        }
+        if (isAlreadyExist(sys_status.available_nodes, node_name))
+        {
+          ret.emplace_back(node_name);
+        }
+      }
+    }
+  }
+  return ret;
+}
+
+std::vector<std::string> HealthAnalyzer::findRootNodes(
+  const std::vector<std::string>& target_nodes)
+{
+  std::vector<std::string> ret;
+  for (const auto& node : target_nodes)
+  {
+    std::vector<std::string> pub_nodes;
+    bool depend_found = false;
+    adjacency_iterator_t vi, vi_end;
+    auto vertex = getTargetNode(node);
+    if (!vertex)
+    {
+      continue;
+    }
+    boost::tie(vi, vi_end) = adjacent_vertices(vertex.get(), depend_graph_);
+    for (; vi != vi_end; ++vi)
+    {
+      if (isAlreadyExist(target_nodes, depend_graph_[*vi].node_name))
+      {
+        depend_found = true;
+      }
+    }
+    if (!depend_found)
+    {
+      ret.emplace_back(node);
+    }
+  }
+  return ret;
+}
+
+autoware_system_msgs::SystemStatus HealthAnalyzer::filterSystemStatus(
+  const autoware_system_msgs::SystemStatus& status)
+{
+  int warn_count = countWarn(status);
+  autoware_system_msgs::SystemStatus filtered_status(status);
+  filtered_status.detect_too_match_warning =
+    (warn_count >= warn_nodes_count_threshold_);
+  filtered_status.node_status.clear();
+  std::vector<std::string> root_nodes(findRootNodes(findWarningNodes(status)));
+  for (const auto& node_status : status.node_status)
+  {
+    if (isAlreadyExist(root_nodes, node_status.node_name))
+    {
+      filtered_status.node_status.emplace_back(node_status);
+    }
+  }
+  return filtered_status;
+}
+
+void HealthAnalyzer::systemStatusCallback(
+    const autoware_system_msgs::SystemStatus::ConstPtr& msg)
+{
+  generateDependGraph(*msg);
+  system_status_summary_pub_.publish(filterSystemStatus(*msg));
+}
+
+void HealthAnalyzer::generateDependGraph(
+  const autoware_system_msgs::SystemStatus& status)
+{
+  depend_graph_ = graph_t();
+  for (const auto& el : status.topic_statistics)
+  {
+    addDepend(el);
+  }
+}
+
+void HealthAnalyzer::writeDot()
+{
+  const std::string path =
+    ros::package::getPath("autoware_health_checker") + "/data/node_depends.dot";
+  std::ofstream f(path.c_str());
+  boost::write_graphviz(f, depend_graph_,
+    boost::make_label_writer(get(&node_property::node_name, depend_graph_)));
+}
+
+boost::optional<HealthAnalyzer::vertex_t>
+  HealthAnalyzer::getTargetNode(const std::string& target_node)
+{
+  vertex_t ret;
+  auto vertex_range = boost::vertices(depend_graph_);
+  for (auto first = vertex_range.first, last = vertex_range.second;
+       first != last; ++first)
+  {
+    vertex_t v = *first;
+    if (depend_graph_[v].node_name == target_node)
+    {
+      ret = v;
+      return ret;
+    }
+  }
+  return boost::none;
+}
+
+void HealthAnalyzer::addDepend(
+  const rosgraph_msgs::TopicStatistics& statistics)
+{
+  if (statistics.node_pub == statistics.node_sub)
+  {
+    return;
+  }
+  vertex_t node_sub;
+  vertex_t node_pub;
+  edge_t topic;
+  auto pub_vertex = getTargetNode(statistics.node_pub);
+  auto sub_vertex = getTargetNode(statistics.node_sub);
+  if (!pub_vertex)
+  {
+    pub_vertex = boost::add_vertex(depend_graph_);
+    depend_graph_[pub_vertex.get()].node_name = statistics.node_pub;
+  }
+  if (!sub_vertex)
+  {
+    sub_vertex = boost::add_vertex(depend_graph_);
+    depend_graph_[sub_vertex.get()].node_name = statistics.node_sub;
+  }
+  bool inserted = false;
+  boost::tie(topic, inserted) =
+    boost::add_edge(sub_vertex.get(), pub_vertex.get(), depend_graph_);
+  depend_graph_[topic].node_sub = statistics.node_sub;
+  depend_graph_[topic].node_pub = statistics.node_pub;
+}

src/ros/catkin/src/autoware_health_checker/src/health_analyzer/health_analyzer_node.cpp

@@ -0,0 +1,32 @@
+/*
+ * 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 <ros/ros.h>
+
+#include <autoware_health_checker/health_analyzer/health_analyzer.h>
+
+int main(int argc, char *argv[])
+{
+  ros::init(argc, argv, "health_analyzer");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+  HealthAnalyzer analyzer(nh, pnh);
+  ros::spin();
+  return 0;
+}

src/ros/catkin/src/autoware_health_checker/src/health_checker/diag_buffer.cpp

@@ -0,0 +1,131 @@
+/*
+ * 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 <string>
+#include <vector>
+#include <algorithm>
+#include <autoware_health_checker/health_checker/diag_buffer.h>
+
+namespace autoware_health_checker
+{
+
+DiagBuffer::DiagBuffer(ErrorKey key, ErrorType type,
+  std::string description, double buffer_duration)
+  : type(type)
+  , description(description)
+  , key_(key)
+  , buffer_duration_(ros::Duration(buffer_duration)) {}
+
+void DiagBuffer::addDiag(autoware_system_msgs::DiagnosticStatus status)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  buffer_[status.level].status.emplace_back(status);
+  updateBuffer();
+}
+
+autoware_system_msgs::DiagnosticStatusArray DiagBuffer::getAndClearData()
+{
+  static const std::array<ErrorLevel, 4> level_array =
+  {
+    AwDiagStatus::ERROR,
+    AwDiagStatus::WARN,
+    AwDiagStatus::OK,
+    AwDiagStatus::UNDEFINED
+  };
+  std::lock_guard<std::mutex> lock(mtx_);
+  updateBuffer();
+  auto data = buffer_.at(AwDiagStatus::FATAL);
+  auto& status = data.status;
+  for (const auto& level : level_array)
+  {
+    const auto& buf_status = buffer_.at(level).status;
+    status.insert(status.end(), buf_status.begin(), buf_status.end());
+  }
+  std::sort(status.begin(), status.end(),
+    std::bind(&DiagBuffer::isOlderTimestamp, this,
+    std::placeholders::_1, std::placeholders::_2));
+  buffer_.clear();
+  return data;
+}
+
+ErrorLevel DiagBuffer::getErrorLevel()
+{
+  static const std::array<ErrorLevel, 4> level_array =
+  {
+    AwDiagStatus::FATAL,
+    AwDiagStatus::ERROR,
+    AwDiagStatus::WARN,
+    AwDiagStatus::OK
+  };
+  std::lock_guard<std::mutex> lock(mtx_);
+  updateBuffer();
+  for (const auto& level : level_array)
+  {
+    if (!buffer_.at(level).status.empty())
+    {
+      return level;
+    }
+  }
+  return AwDiagStatus::OK;
+}
+
+// filter data from timestamp and level
+autoware_system_msgs::DiagnosticStatusArray
+DiagBuffer::filterBuffer(ros::Time now, ErrorLevel level)
+{
+  autoware_system_msgs::DiagnosticStatusArray filtered_data;
+  if (buffer_.count(level) != 0)
+  {
+    filtered_data = buffer_.at(level);
+  }
+  autoware_system_msgs::DiagnosticStatusArray ret;
+  for (const auto& data : filtered_data.status)
+  {
+    if ((data.header.stamp + buffer_duration_) > now)
+    {
+      ret.status.emplace_back(data);
+    }
+  }
+  return ret;
+}
+
+void DiagBuffer::updateBuffer()
+{
+  static const std::array<ErrorLevel, 5> level_array =
+  {
+    AwDiagStatus::FATAL,
+    AwDiagStatus::ERROR,
+    AwDiagStatus::WARN,
+    AwDiagStatus::OK,
+    AwDiagStatus::UNDEFINED
+  };
+  ros::Time now = ros::Time::now();
+  for (const auto& level : level_array)
+  {
+    buffer_[level] = filterBuffer(now, level);
+  }
+}
+
+bool DiagBuffer::isOlderTimestamp(
+    const autoware_system_msgs::DiagnosticStatus &a,
+    const autoware_system_msgs::DiagnosticStatus &b)
+{
+  return a.header.stamp < b.header.stamp;
+}
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/src/health_checker/health_checker.cpp

@@ -0,0 +1,301 @@
+/*
+ * 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 <string>
+#include <vector>
+#include <autoware_health_checker/health_checker/health_checker.h>
+
+namespace autoware_health_checker
+{
+HealthChecker::HealthChecker(ros::NodeHandle nh, ros::NodeHandle pnh)
+  : value_manager_(nh, pnh)
+  , node_activated_(false)
+  , nh_(nh)
+  , pnh_(pnh)
+  , is_shutdown_(false)
+{
+  status_pub_ =
+    nh_.advertise<autoware_system_msgs::NodeStatus>("node_status", 10);
+}
+
+HealthChecker::~HealthChecker()
+{
+  is_shutdown_.store(true);
+  if (node_status_publish_thread_.joinable())
+  {
+    node_status_publish_thread_.join();
+  }
+}
+
+void HealthChecker::publishStatus()
+{
+  const int loop_usec =
+    std::round(1.0 / autoware_health_checker::NODE_STATUS_UPDATE_RATE * 1e6);
+  const std::string node_name = ros::this_node::getName();
+
+  auto prev_time = std::chrono::system_clock::now();
+  ros::Time prev_ros_time = ros::Time::now();
+  while (ros::ok() && !is_shutdown_.load())
+  {
+    const auto until_time = prev_time + std::chrono::microseconds(loop_usec);
+    std::this_thread::sleep_until(until_time);
+    prev_time = until_time;
+    // Pause publish if time is stopped in simulation mode.
+    const ros::Time now = ros::Time::now();
+    if (prev_ros_time == now)
+    {
+      continue;
+    }
+    prev_ros_time = now;
+
+    autoware_system_msgs::NodeStatus status;
+    status.node_name = node_name;
+
+    status.node_activated = node_activated_;
+    status.header.stamp = now;
+    std::lock_guard<std::mutex> lock(mtx_);
+    const auto checker_keys = getRateCheckerKeys();
+    // iterate Rate checker and publish rate_check result
+    for (const auto& key : checker_keys)
+    {
+      const auto result = rate_checkers_[key]->getErrorLevelAndRate();
+      if (result)
+      {
+        AwDiagStatusArray diag_array;
+        AwDiagStatus diag = setValueCommon(
+          key, result->second, rate_checkers_.at(key)->description);
+        diag.header.stamp = now;
+        diag.level = result->first;
+        diag.type = AwDiagStatus::UNEXPECTED_RATE;
+        diag_array.status.emplace_back(diag);
+        status.status.emplace_back(diag_array);
+      }
+    }
+    // iterate Diagnostic Buffer and publish all diagnostic data
+    const auto keys = getKeys();
+    for (const auto& key : keys)
+    {
+      status.status.emplace_back(diag_buffers_.at(key)->getAndClearData());
+    }
+    status_pub_.publish(status);
+  }
+}
+
+ErrorLevel HealthChecker::SET_DIAG_STATUS(
+  const autoware_system_msgs::DiagnosticStatus& status)
+{
+  value_manager_.addCandidate(status.key);
+  if (value_manager_.isNotFound(status.key))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  auto identify = [&status](const ErrorLevel target_level)
+  {
+    return (status.level == target_level);
+  };
+  if (!identify(AwDiagStatus::OK) && !identify(AwDiagStatus::WARN) &&
+    !identify(AwDiagStatus::ERROR) && !identify(AwDiagStatus::FATAL))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  std::lock_guard<std::mutex> lock(mtx_);
+  addNewBuffer(status.key, status.type, status.description);
+  diag_buffers_.at(status.key)->addDiag(status);
+  return status.level;
+}
+
+ErrorLevel HealthChecker::CHECK_TRUE(
+  const ErrorKey& key, const bool value,
+  const ErrorLevel level, const std::string& description)
+{
+  value_manager_.addCandidate(key);
+  if (value_manager_.isNotFound(key))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  AwDiagStatus status = setValueCommon(key, value, description);
+  status.level = level;
+  status.type = status.INVALID_VALUE;
+  return SET_DIAG_STATUS(status);
+}
+
+void HealthChecker::ENABLE()
+{
+  node_status_publish_thread_ = std::thread(&HealthChecker::publishStatus, this);
+}
+
+std::vector<ErrorKey> HealthChecker::getKeys()
+{
+  std::vector<ErrorKey> keys;
+  const auto checked = getRateCheckerKeys();
+  for (const auto& buf : diag_buffers_)
+  {
+    const auto res = std::find(checked.begin(), checked.end(), buf.first);
+    if (res == checked.end())
+    {
+      keys.emplace_back(buf.first);
+    }
+  }
+  return keys;
+}
+
+std::vector<ErrorKey> HealthChecker::getRateCheckerKeys()
+{
+  std::vector<ErrorKey> keys;
+  for (const auto& checker : rate_checkers_)
+  {
+    keys.emplace_back(checker.first);
+  }
+  return keys;
+}
+
+bool HealthChecker::keyExist(const ErrorKey& key) const
+{
+  return (diag_buffers_.count(key) != 0);
+}
+
+// add New Diagnostic Buffer if the key does not exist
+bool HealthChecker::addNewBuffer(
+  const ErrorKey& key, const ErrorType type, const std::string& description)
+{
+  if (keyExist(key))
+  {
+    return false;
+  }
+  diag_buffers_[key] = std::make_unique<DiagBuffer>(key, type, description,
+    autoware_health_checker::BUFFER_DURATION);
+  return true;
+}
+
+ErrorLevel HealthChecker::CHECK_MIN_VALUE(const ErrorKey& key,
+  const double value, const double warn_value, const double error_value,
+  const double fatal_value, const std::string& description)
+{
+  value_manager_.addCandidate(key);
+  if (value_manager_.isNotFound(key))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  static const ThreshType thresh_type = "min";
+  value_manager_.setDefaultValue(
+    key, thresh_type, warn_value, error_value, fatal_value);
+  auto identify = [key, value, thresh_type, this](ErrorLevel level)
+  {
+    return (value < value_manager_.getValue(key, thresh_type, level).get());
+  };
+  AwDiagStatus new_status = setValueCommon(key, value, description);
+  new_status.level = identify(AwDiagStatus::FATAL) ? AwDiagStatus::FATAL :
+    identify(AwDiagStatus::ERROR) ? AwDiagStatus::ERROR :
+    identify(AwDiagStatus::WARN) ? AwDiagStatus::WARN :
+    AwDiagStatus::OK;
+  new_status.type = AwDiagStatus::OUT_OF_RANGE;
+  return SET_DIAG_STATUS(new_status);
+}
+
+ErrorLevel HealthChecker::CHECK_MAX_VALUE(const ErrorKey& key,
+  const double value, const double warn_value, const double error_value,
+  const double fatal_value, const std::string& description)
+{
+  value_manager_.addCandidate(key);
+  if (value_manager_.isNotFound(key))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  static const ThreshType thresh_type = "max";
+  value_manager_.setDefaultValue(
+    key, thresh_type, warn_value, error_value, fatal_value);
+  auto identify = [key, value, thresh_type, this](ErrorLevel level)
+  {
+    return (value > value_manager_.getValue(key, thresh_type, level).get());
+  };
+  AwDiagStatus new_status = setValueCommon(key, value, description);
+  new_status.level = identify(AwDiagStatus::FATAL) ? AwDiagStatus::FATAL :
+    identify(AwDiagStatus::ERROR) ? AwDiagStatus::ERROR :
+    identify(AwDiagStatus::WARN) ? AwDiagStatus::WARN :
+    AwDiagStatus::OK;
+  new_status.type = AwDiagStatus::OUT_OF_RANGE;
+  return SET_DIAG_STATUS(new_status);
+}
+
+ErrorLevel HealthChecker::CHECK_RANGE(const ErrorKey& key,
+  const double value, const MinMax warn_value, const MinMax error_value,
+  const MinMax fatal_value, const std::string& description)
+{
+  value_manager_.addCandidate(key);
+  if (value_manager_.isNotFound(key))
+  {
+    return AwDiagStatus::UNDEFINED;
+  }
+  value_manager_.setDefaultValue(key, "min", warn_value.first,
+    error_value.first, fatal_value.first);
+  value_manager_.setDefaultValue(key, "max", warn_value.second,
+    error_value.second, fatal_value.second);
+  auto identify = [key, value, this](ErrorLevel level)
+  {
+    return (value < value_manager_.getValue(key, "min", level).get()) ||
+      (value > value_manager_.getValue(key, "max", level).get());
+  };
+  AwDiagStatus new_status = setValueCommon(key, value, description);
+  new_status.level = identify(AwDiagStatus::FATAL) ? AwDiagStatus::FATAL :
+    identify(AwDiagStatus::ERROR) ? AwDiagStatus::ERROR :
+    identify(AwDiagStatus::WARN) ? AwDiagStatus::WARN :
+    AwDiagStatus::OK;
+  new_status.type = AwDiagStatus::OUT_OF_RANGE;
+  return SET_DIAG_STATUS(new_status);
+}
+
+void HealthChecker::CHECK_RATE(const ErrorKey& key,
+  const double warn_rate, const double error_rate,
+  const double fatal_rate, const std::string& description)
+{
+  value_manager_.addCandidate(key);
+  if (value_manager_.isNotFound(key))
+  {
+    return;
+  }
+  std::lock_guard<std::mutex> lock(mtx_);
+  if (!keyExist(key))
+  {
+    value_manager_.setDefaultValue(
+      key, "rate", warn_rate, error_rate, fatal_rate);
+    rate_checkers_[key] = std::make_unique<RateChecker>(
+      autoware_health_checker::BUFFER_DURATION,
+      warn_rate, error_rate, fatal_rate, description);
+  }
+  rate_checkers_[key]->setRate(
+    value_manager_.getValue(key, "rate", AwDiagStatus::WARN).get(),
+    value_manager_.getValue(key, "rate", AwDiagStatus::ERROR).get(),
+    value_manager_.getValue(key, "rate", AwDiagStatus::FATAL).get());
+  rate_checkers_[key]->check();
+  addNewBuffer(key, AwDiagStatus::UNEXPECTED_RATE, description);
+}
+
+template <typename T>
+  autoware_system_msgs::DiagnosticStatus HealthChecker::setValueCommon(
+    const ErrorKey& key, const T& value, const std::string& desc)
+{
+  AwDiagStatus new_status;
+  new_status.header.stamp = ros::Time::now();
+  new_status.key = key;
+  new_status.value = valueToJson(value);
+  new_status.description = desc;
+  return new_status;
+}
+
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/src/health_checker/param_manager.cpp

@@ -0,0 +1,86 @@
+/*
+ * Copyright 2015-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.
+ */
+
+#include <set>
+#include <autoware_health_checker/health_checker/param_manager.h>
+
+namespace autoware_health_checker
+{
+
+ParamManager::ParamManager(ros::NodeHandle nh, ros::NodeHandle pnh)
+  : nh_(nh), pnh_(pnh)
+{
+  nh_.getParam("health_checker", params_);
+  get_timer_ =
+    nh_.createTimer(ros::Duration(1.0), &ParamManager::getParams, this);
+  set_timer_ =
+    nh_.createTimer(ros::Duration(1.0), &ParamManager::setParams, this);
+}
+
+void ParamManager::addCandidate(const ErrorKey& key)
+{
+  std::lock_guard<std::mutex> lock(key_mtx_);
+  cached_candidate_key_.emplace(key);
+}
+
+bool ParamManager::isNotFound(const ErrorKey& key)
+{
+  std::lock_guard<std::mutex> lock(param_mtx_);
+  return !params_.valid() || !params_.hasMember(key);
+}
+
+bool ParamManager::isNotFound(const ErrorKey& ns, const ErrorKey& key)
+{
+  std::lock_guard<std::mutex> lock(param_mtx_);
+  return !params_.valid() || !params_.hasMember(ns)
+    ? true : !params_[ns].hasMember(key);
+}
+
+const XmlRpc::XmlRpcValue& ParamManager::getParams()
+{
+  std::lock_guard<std::mutex> lock(param_mtx_);
+  return params_;
+}
+
+void ParamManager::getParams(const ros::TimerEvent& event)
+{
+  XmlRpc::XmlRpcValue param;
+  nh_.getParam("health_checker", param);
+  // Even if it is delayed at the time of getParam(),
+  // it is necessary to prevent the delay from affecting getValue().
+  std::lock_guard<std::mutex> lock(param_mtx_);
+  params_ = param;
+}
+
+void ParamManager::setParams(const ros::TimerEvent& event)
+{
+  std::set<ErrorKey> keys;
+  {
+    std::lock_guard<std::mutex> lock(key_mtx_);
+    keys = cached_candidate_key_;
+    cached_candidate_key_.clear();
+  }
+  static std::set<ErrorKey> previous_keys;
+  for (const auto& key : keys)
+  {
+    if (previous_keys.count(key) == 0)
+    {
+      nh_.setParam("diag_reference/" + key, "default");
+    }
+  }
+  previous_keys.insert(keys.begin(), keys.end());
+}
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/src/health_checker/rate_checker.cpp

@@ -0,0 +1,101 @@
+/*
+ * 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 <string>
+#include <vector>
+#include <autoware_health_checker/health_checker/rate_checker.h>
+
+namespace autoware_health_checker
+{
+RateChecker::RateChecker(
+  double buffer_duration, double warn_rate, double error_rate,
+  double fatal_rate, std::string description)
+  : buffer_duration_(buffer_duration), warn_rate_(warn_rate),
+    error_rate_(error_rate), fatal_rate_(fatal_rate),
+    description(description), start_time_(ros::Time::now()) {}
+
+boost::optional<LevelRatePair> RateChecker::getErrorLevelAndRate()
+{
+  const auto rate = getRate();
+  if (!rate)
+  {
+    return boost::none;
+  }
+  const ErrorLevel level =
+    (rate.get() < fatal_rate_) ? AwDiagStatus::FATAL :
+    (rate.get() < error_rate_) ? AwDiagStatus::ERROR :
+    (rate.get() < warn_rate_) ? AwDiagStatus::WARN :
+    AwDiagStatus::OK;
+  return std::make_pair(level, rate.get());
+}
+
+boost::optional<ErrorLevel> RateChecker::getErrorLevel()
+{
+  auto level_and_rate = getErrorLevelAndRate();
+  if (!level_and_rate)
+  {
+    return boost::none;
+  }
+  return level_and_rate.get().first;
+}
+
+void RateChecker::setRate(
+  double warn_rate, double error_rate, double fatal_rate)
+{
+  update();
+  std::lock_guard<std::mutex> lock(mtx_);
+  warn_rate_ = warn_rate;
+  error_rate_ = error_rate;
+  fatal_rate_ = fatal_rate;
+}
+
+void RateChecker::check()
+{
+  update();
+  std::lock_guard<std::mutex> lock(mtx_);
+  data_.emplace_back(ros::Time::now());
+}
+
+void RateChecker::update()
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  std::vector<ros::Time> buffer;
+  for (const auto& el : data_)
+  {
+    if (el + ros::Duration(buffer_duration_) > ros::Time::now())
+    {
+      buffer.emplace_back(el);
+    }
+  }
+  data_ = buffer;
+}
+
+boost::optional<double> RateChecker::getRate()
+{
+  boost::optional<double> rate;
+  if (ros::Time::now() < start_time_ + ros::Duration(buffer_duration_))
+  {
+    return boost::none;
+  }
+  update();
+  std::lock_guard<std::mutex> lock(mtx_);
+  rate = data_.size() / buffer_duration_;
+  return rate;
+}
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/src/health_checker/value_manager.cpp

@@ -0,0 +1,57 @@
+/*
+ * 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 <string>
+#include <autoware_health_checker/health_checker/value_manager.h>
+namespace autoware_health_checker
+{
+ValueManager::ValueManager(ros::NodeHandle nh, ros::NodeHandle pnh)
+  : ParamManager(nh, pnh) {}
+
+void ValueManager::setDefaultValue(
+  const ErrorKey& key, const ThreshType& thresh_type,
+  const double warn_value, const double error_value, const double fatal_value)
+{
+  const auto category = std::make_pair(key, thresh_type);
+  error_details_[std::make_pair(category, AwDiagStatus::WARN)] = warn_value;
+  error_details_[std::make_pair(category, AwDiagStatus::ERROR)] = error_value;
+  error_details_[std::make_pair(category, AwDiagStatus::FATAL)] = fatal_value;
+}
+
+boost::optional<double> ValueManager::getValue(const ErrorKey& key,
+  const ThreshType& thresh_type, const ErrorLevel level)
+{
+  const std::string level_name =
+    (level == AwDiagStatus::WARN) ? "warn" :
+    (level == AwDiagStatus::ERROR) ? "error" :
+    (level == AwDiagStatus::FATAL) ? "fatal" :
+    std::string();
+  auto diag_params = getParams();
+  if (level_name.empty() || !diag_params.hasMember(key))
+  {
+    return boost::none;
+  }
+  const bool has_level = diag_params[key].hasMember(thresh_type) ?
+    diag_params[key][thresh_type].hasMember(level_name) : false;
+  const auto category = std::make_pair(key, thresh_type);
+  return (has_level) ?
+    static_cast<double>(diag_params[key][thresh_type][level_name]) :
+    error_details_.at(std::make_pair(category, level));
+}
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/src/system_status_subscriber/system_status_subscriber.cpp

@@ -0,0 +1,49 @@
+/*
+ * 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 <autoware_health_checker/system_status_subscriber/system_status_subscriber.h>
+
+namespace autoware_health_checker
+{
+SystemStatusSubscriber::SystemStatusSubscriber(
+  ros::NodeHandle nh, ros::NodeHandle pnh)
+  : nh_(nh), pnh_(pnh) {}
+
+void SystemStatusSubscriber::enable()
+{
+  status_sub_ = nh_.subscribe(
+    "system_status", 10, &SystemStatusSubscriber::systemStatusCallback, this);
+}
+
+void SystemStatusSubscriber::systemStatusCallback(
+  const autoware_system_msgs::SystemStatus::ConstPtr msg)
+{
+  for (auto& func : functions_)
+  {
+    auto pmsg = std::make_shared<autoware_system_msgs::SystemStatus>(*msg);
+    func(pmsg);
+  }
+}
+
+void SystemStatusSubscriber::addCallback(
+  std::function<void(std::shared_ptr<autoware_system_msgs::SystemStatus>)> func)
+{
+  functions_.emplace_back(func);
+}
+}  // namespace autoware_health_checker

src/ros/catkin/src/autoware_health_checker/test/src/test_autoware_health_checker.cpp

@@ -0,0 +1,242 @@
+/*
+ * 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 <autoware_health_checker/health_checker/health_checker.h>
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <vector>
+#include <utility>
+
+using AwDiagStatus = autoware_system_msgs::DiagnosticStatus;
+using ErrorLevel = autoware_health_checker::ErrorLevel;
+template <class T>
+using InputAndResult = std::vector<std::pair<T, ErrorLevel>>;
+
+class AutowareHealthCheckerTestClass
+{
+public:
+  AutowareHealthCheckerTestClass() : pnh("~") {}
+  std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr;
+  ros::NodeHandle pnh;
+  ros::NodeHandle nh;
+  void init()
+  {
+    ros::param::set("health_checker/test", "default");
+    health_checker_ptr =
+      std::make_shared<autoware_health_checker::HealthChecker>(nh, pnh);
+  }
+};
+
+class AutowareHealthCheckerTestSuite : public ::testing::Test
+{
+public:
+  AutowareHealthCheckerTestSuite() {}
+
+  ~AutowareHealthCheckerTestSuite() {}
+  AutowareHealthCheckerTestClass test_obj_;
+
+protected:
+  virtual void SetUp()
+  {
+    test_obj_.init();
+  }
+  virtual void TearDown() {}
+};
+
+autoware_health_checker::ErrorLevel test_function(double value)
+{
+  return
+    (value == 0.0) ? AwDiagStatus::FATAL :
+    (value == 1.0) ? AwDiagStatus::ERROR :
+    (value == 2.0) ? AwDiagStatus::WARN : AwDiagStatus::OK;
+};
+
+boost::property_tree::ptree test_value_json_func(double value)
+{
+  boost::property_tree::ptree tree;
+  tree.put("value", value);
+  return tree;
+};
+
+/*
+  test for value check function
+*/
+TEST_F(AutowareHealthCheckerTestSuite, CHECK_VALUE)
+{
+  using CheckLevel = std::function<ErrorLevel(double value)>;
+  using CheckVal = std::function<boost::property_tree::ptree(double value)>;
+  CheckLevel check_func = test_function;
+  CheckVal check_value_json_func = test_value_json_func;
+
+  const InputAndResult<double> dataset =
+  {
+    std::make_pair(0.0, AwDiagStatus::FATAL),
+    std::make_pair(1.0, AwDiagStatus::ERROR),
+    std::make_pair(2.0, AwDiagStatus::WARN),
+    std::make_pair(-1.0, AwDiagStatus::OK)
+  };
+  for (const auto& data : dataset)
+  {
+    auto ret_value = test_obj_.health_checker_ptr->CHECK_VALUE(
+      "test", data.first, check_func, check_value_json_func, "test");
+    ASSERT_EQ(ret_value, data.second)
+      << "CHECK_VALUE function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+  auto value = check_value_json_func(0.0).get_optional<double>("value");
+  ASSERT_EQ(value.get(), 0.0)
+    << "Failed to get json value."
+    << "It should be 0.0";
+}
+
+/*
+  test for minimum value check function
+*/
+TEST_F(AutowareHealthCheckerTestSuite, CHECK_MIN_VALUE)
+{
+  const InputAndResult<double> dataset =
+  {
+    std::make_pair(1.0, AwDiagStatus::FATAL),
+    std::make_pair(3.0, AwDiagStatus::ERROR),
+    std::make_pair(5.0, AwDiagStatus::WARN),
+    std::make_pair(7.0, AwDiagStatus::OK)
+  };
+  for (const auto& data : dataset)
+  {
+    auto ret_min = test_obj_.health_checker_ptr->CHECK_MIN_VALUE(
+      "test", data.first, 6, 4, 2, "test");
+    ASSERT_EQ(ret_min, data.second)
+      << "CHECK_MIN_VALUE function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+}
+
+/*
+  test for maximum value check function
+*/
+TEST_F(AutowareHealthCheckerTestSuite, CHECK_MAX_VALUE)
+{
+  const InputAndResult<double> dataset =
+  {
+    std::make_pair(7.0, AwDiagStatus::FATAL),
+    std::make_pair(5.0, AwDiagStatus::ERROR),
+    std::make_pair(3.0, AwDiagStatus::WARN),
+    std::make_pair(1.0, AwDiagStatus::OK)
+  };
+  for (const auto& data : dataset)
+  {
+    auto ret_max = test_obj_.health_checker_ptr->CHECK_MAX_VALUE(
+      "test", data.first, 2, 4, 6, "test");
+    ASSERT_EQ(ret_max, data.second)
+      << "CHECK_MAX_VALUE function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+}
+
+/*
+  test for range check functions
+*/
+TEST_F(AutowareHealthCheckerTestSuite, CHECK_RANGE)
+{
+  const std::pair<double, double> warn_range = std::make_pair(2.0, 4.0);
+  const std::pair<double, double> error_range = std::make_pair(1.0, 5.0);
+  const std::pair<double, double> fatal_range = std::make_pair(0.0, 6.0);
+  const InputAndResult<double> dataset =
+  {
+    std::make_pair(7.0, AwDiagStatus::FATAL),
+    std::make_pair(5.5, AwDiagStatus::ERROR),
+    std::make_pair(4.5, AwDiagStatus::WARN),
+    std::make_pair(3.0, AwDiagStatus::OK)
+  };
+
+  for (const auto& data : dataset)
+  {
+    auto ret_range = test_obj_.health_checker_ptr->CHECK_RANGE(
+      "test", data.first, warn_range, error_range, fatal_range, "test");
+    ASSERT_EQ(ret_range, data.second)
+      << "CHECK_RANGE function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+}
+
+/*
+  test for bool check functions
+*/
+TEST_F(AutowareHealthCheckerTestSuite, CHECK_TRUE)
+{
+  const InputAndResult<bool> dataset =
+  {
+    std::make_pair(true, AwDiagStatus::FATAL),
+    std::make_pair(false, AwDiagStatus::ERROR),
+    std::make_pair(true, AwDiagStatus::WARN),
+    std::make_pair(false, AwDiagStatus::OK)
+  };
+  for (const auto& data : dataset)
+  {
+    auto ret_bool = test_obj_.health_checker_ptr->CHECK_TRUE(
+      "test", data.first, data.second, "test");
+    ASSERT_EQ(ret_bool, data.second)
+      << "CHECK_TRUE function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+}
+
+/*
+  test for set diag function
+*/
+TEST_F(AutowareHealthCheckerTestSuite, SET_DIAG_STATUS)
+{
+  const InputAndResult<ErrorLevel> dataset =
+  {
+    std::make_pair(AwDiagStatus::FATAL, AwDiagStatus::FATAL),
+    std::make_pair(AwDiagStatus::ERROR, AwDiagStatus::ERROR),
+    std::make_pair(AwDiagStatus::WARN, AwDiagStatus::WARN),
+    std::make_pair(AwDiagStatus::OK, AwDiagStatus::OK)
+  };
+  autoware_system_msgs::DiagnosticStatus status;
+  status.key = "test";
+  for (const auto& data : dataset)
+  {
+    status.level = data.first;
+    auto ret_diag = test_obj_.health_checker_ptr->SET_DIAG_STATUS(status);
+    ASSERT_EQ(ret_diag, data.second)
+      << "SET_DIAG_STATUS function returns invalid value."
+      << "It should be " << static_cast<int>(data.second);
+  }
+}
+
+/*
+  test for node status
+*/
+TEST_F(AutowareHealthCheckerTestSuite, NODE_STATUS)
+{
+  test_obj_.health_checker_ptr->NODE_ACTIVATE();
+  auto ret_active = test_obj_.health_checker_ptr->getNodeStatus();
+  ASSERT_EQ(ret_active, true) << "The value must be true";
+  test_obj_.health_checker_ptr->NODE_DEACTIVATE();
+  auto ret_inactive = test_obj_.health_checker_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");
+  ros::NodeHandle nh;
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/autoware_health_checker/test/test_autoware_health_checker.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test-autoware_health_checker" pkg="autoware_health_checker" type="test-autoware_health_checker" name="test"/>
+</launch>

src/ros/catkin/src/autoware_system_msgs/CHANGELOG.rst

@@ -0,0 +1,23 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_system_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.13.0 (2019-12-03)
+-------------------
+* Update emergency category in DiagnosticStatus message
+* Update package.xml files to Format 2.
+* Contributors: Joshua Whitley, Yuma Nihei
+
+1.12.0 (2019-07-12)
+-------------------
+
+1.11.0 (2019-03-21)
+-------------------
+* Revert "Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)" (`#2037 <https://github.com/CPFL/Autoware/issues/2037>`_)
+  This reverts commit e4187a7138eb90ad6f119eb35f824b16465aefda.
+  Reverts `#2012 <https://github.com/CPFL/Autoware/issues/2012>`_
+  Merged without adequate description of the bug or fixes made
+* Fix/health checker (`#2012 <https://github.com/CPFL/Autoware/issues/2012>`_)
+* Feature/autoware health analyzer (`#2004 <https://github.com/CPFL/Autoware/issues/2004>`_)
+* Feature/autoware health checker (`#1943 <https://github.com/CPFL/Autoware/issues/1943>`_)
+* Contributors: Geoffrey Biggs, Masaya Kataoka

src/ros/catkin/src/autoware_system_msgs/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(autoware_system_msgs)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  rosgraph_msgs
+  std_msgs
+)
+
+add_message_files(
+  FILES
+    DiagnosticStatus.msg
+    DiagnosticStatusArray.msg
+    HardwareStatus.msg
+    NodeStatus.msg
+    SystemStatus.msg
+)
+
+generate_messages(DEPENDENCIES
+  rosgraph_msgs
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    rosgraph_msgs
+    std_msgs
+)

src/ros/catkin/src/autoware_system_msgs/msg/DiagnosticStatus.msg

@@ -0,0 +1,23 @@
+Header header
+
+string key
+
+string value
+
+string description
+
+uint8 UNDEFINED = 0
+
+uint8 type
+uint8 OUT_OF_RANGE = 1
+uint8 UNEXPECTED_RATE = 2
+uint8 INVALID_VALUE = 3
+uint8 INTERNAL = 4
+uint8 FUNCTIONAL = 100
+uint8 HARDWARE = 200
+
+uint8 level
+uint8 OK = 1
+uint8 WARN = 2
+uint8 ERROR = 3
+uint8 FATAL = 4

src/ros/catkin/src/autoware_system_msgs/msg/DiagnosticStatusArray.msg

@@ -0,0 +1 @@
+autoware_system_msgs/DiagnosticStatus[] status

src/ros/catkin/src/autoware_system_msgs/msg/HardwareStatus.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+string hardware_name
+autoware_system_msgs/DiagnosticStatusArray[] status

src/ros/catkin/src/autoware_system_msgs/msg/NodeStatus.msg

@@ -0,0 +1,4 @@
+Header header
+string node_name
+bool node_activated
+autoware_system_msgs/DiagnosticStatusArray[] status

src/ros/catkin/src/autoware_system_msgs/msg/SystemStatus.msg

@@ -0,0 +1,6 @@
+Header header
+string[] available_nodes
+bool detect_too_match_warning
+autoware_system_msgs/NodeStatus[] node_status
+autoware_system_msgs/HardwareStatus[] hardware_status
+rosgraph_msgs/TopicStatistics[] topic_statistics

src/ros/catkin/src/autoware_system_msgs/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autoware_system_msgs</name>
+  <version>1.13.0</version>
+  <description>The autoware_system_msgs package</description>
+  <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+  <depend>rosgraph_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

src/ros/catkin/src/libwaypoint_follower/CMakeLists.txt

@@ -0,0 +1,94 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(libwaypoint_follower)
+
+find_package(autoware_build_flags)
+
+find_package(catkin REQUIRED COMPONENTS
+  amathutils_lib
+  autoware_msgs
+  geometry_msgs
+  roscpp
+  roslint
+  rosunit
+  std_msgs
+)
+
+find_package(Eigen3 QUIET)
+
+if (NOT EIGEN3_FOUND)
+  # Fallback to cmake_modules
+  find_package(cmake_modules REQUIRED)
+  find_package(Eigen REQUIRED)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+  # Possibly map additional variables to the EIGEN3_ prefix.
+else ()
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif ()
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES libwaypoint_follower
+  CATKIN_DEPENDS
+    amathutils_lib
+    autoware_msgs
+    geometry_msgs
+    std_msgs
+)
+
+SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+
+include_directories(libwaypoint_follower
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+add_library(libwaypoint_follower src/libwaypoint_follower.cpp
+  src/pure_pursuit.cpp)
+add_dependencies(libwaypoint_follower ${catkin_EXPORTED_TARGETS})
+target_link_libraries(libwaypoint_follower ${catkin_LIBRARIES})
+
+## Install executables and/or libraries
+install(TARGETS libwaypoint_follower
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+file(GLOB_RECURSE ROSLINT_FILES
+  LIST_DIRECTORIES false
+  *.cpp *.h *.hpp
+)
+
+list(APPEND ROSLINT_CPP_OPTS "--extensions=cc,h,hpp,cpp,cu,cuh" "--filter=-build/c++11,-runtime/references")
+roslint_cpp(${ROSLINT_FILES})
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(test-libwaypoint_follower
+    test/test_libwaypoint_follower.test
+    test/src/test_libwaypoint_follower.cpp
+    src/libwaypoint_follower.cpp
+  )
+  add_dependencies(test-libwaypoint_follower ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(test-libwaypoint_follower
+    ${catkin_LIBRARIES}
+  )
+  add_rostest_gtest(test-pure_pursuit
+    test/test_pure_pursuit.test
+    test/src/test_pure_pursuit.cpp
+    src/libwaypoint_follower.cpp
+    src/pure_pursuit.cpp
+  )  
+  add_dependencies(test-pure_pursuit ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(test-pure_pursuit
+    ${catkin_LIBRARIES}
+  )
+  roslint_add_test()
+endif ()

src/ros/catkin/src/libwaypoint_follower/include/libwaypoint_follower/libwaypoint_follower.h

@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015-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.
+ */
+
+#ifndef LIBWAYPOINT_FOLLOWER_LIBWAYPOINT_FOLLOWER_H
+#define LIBWAYPOINT_FOLLOWER_LIBWAYPOINT_FOLLOWER_H
+
+#define EIGEN_MPL2_ONLY
+#include <tf/transform_datatypes.h>
+#include <tf2/utils.h>
+
+// C++ header
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <utility>
+#include <vector>
+
+// ROS header
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+#include <autoware_msgs/Lane.h>
+
+constexpr double ERROR = 1e-6;
+
+enum class LaneDirection : int
+{
+  Backward = -1,
+  Forward = 1,
+  Error = 0
+};
+
+class WayPoints
+{
+protected:
+  autoware_msgs::Lane current_waypoints_;
+
+public:
+  void setPath(const autoware_msgs::Lane& waypoints)
+  {
+    current_waypoints_ = waypoints;
+  }
+  int getSize() const;
+  bool isEmpty() const
+  {
+    return current_waypoints_.waypoints.empty();
+  };
+  double getInterval() const;
+  geometry_msgs::Point getWaypointPosition(int waypoint) const;
+  geometry_msgs::Quaternion getWaypointOrientation(int waypoint) const;
+  geometry_msgs::Pose getWaypointPose(int waypoint) const;
+  double getWaypointVelocityMPS(int waypoint) const;
+  autoware_msgs::Lane getCurrentWaypoints() const
+  {
+    return current_waypoints_;
+  }
+  bool inDrivingDirection(int waypoint, geometry_msgs::Pose current_pose) const;
+};
+
+// inline function (less than 10 lines )
+inline double kmph2mps(double velocity_kmph)
+{
+  return (velocity_kmph * 1000) / (60 * 60);
+}
+inline double mps2kmph(double velocity_mps)
+{
+  return (velocity_mps * 60 * 60) / 1000;
+}
+
+tf::Vector3 point2vector(geometry_msgs::Point point);                         // convert point to vector
+geometry_msgs::Point vector2point(tf::Vector3 vector);                        // convert vector to point
+tf::Vector3 rotateUnitVector(tf::Vector3 unit_vector, double degree);         // rotate unit vector by degree
+geometry_msgs::Point rotatePoint(geometry_msgs::Point point, double degree);  // rotate point vector by degree
+
+double DecelerateVelocity(double distance, double prev_velocity);
+geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point,
+                                            geometry_msgs::Pose current_pose);  // transform point into the coordinate
+                                                                                // of current_pose
+geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point,
+                                            geometry_msgs::Pose current_pose);  // transform point into the global
+                                                                                // coordinate
+double getPlaneDistance(geometry_msgs::Point target1,
+                        geometry_msgs::Point target2);  // get 2 dimentional distance between target 1 and target 2
+LaneDirection getLaneDirection(const autoware_msgs::Lane& current_path);
+LaneDirection getLaneDirectionByPosition(const autoware_msgs::Lane& current_path);
+LaneDirection getLaneDirectionByVelocity(const autoware_msgs::Lane& current_path);
+int getClosestWaypoint(const autoware_msgs::Lane& current_path, geometry_msgs::Pose current_pose);
+bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double* a, double* b, double* c);
+double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double sa, double b, double c);
+double getRelativeAngle(geometry_msgs::Pose waypoint_pose, geometry_msgs::Pose vehicle_pose);
+double calcCurvature(const geometry_msgs::Point &target, const geometry_msgs::Pose &curr_pose);
+double calcDistSquared2D(const geometry_msgs::Point &p, const geometry_msgs::Point &q);
+double calcLateralError2D(const geometry_msgs::Point &a_start, const geometry_msgs::Point &a_end,
+                          const geometry_msgs::Point &b);
+double calcRadius(const geometry_msgs::Point &target, const geometry_msgs::Pose &current_pose);
+std::vector<geometry_msgs::Pose> extractPoses(const autoware_msgs::Lane &lane);
+std::vector<geometry_msgs::Pose> extractPoses(const std::vector<autoware_msgs::Waypoint> &wps);
+std::pair<bool, int32_t> findClosestIdxWithDistAngThr(const std::vector<geometry_msgs::Pose> &curr_ps,
+                                                      const geometry_msgs::Pose &curr_pose,
+                                                      double dist_thr = 3.0,
+                                                      double angle_thr = M_PI_2);
+geometry_msgs::Quaternion getQuaternionFromYaw(const double &_yaw);
+bool isDirectionForward(const std::vector<geometry_msgs::Pose> &poses);
+double normalizeEulerAngle(double euler);
+geometry_msgs::Point transformToAbsoluteCoordinate2D(const geometry_msgs::Point &point,
+                                                                      const geometry_msgs::Pose &current_pose);
+geometry_msgs::Point transformToAbsoluteCoordinate3D(const geometry_msgs::Point &point,
+                                                                      const geometry_msgs::Pose &origin);
+geometry_msgs::Point transformToRelativeCoordinate2D(const geometry_msgs::Point &point,
+                                                                      const geometry_msgs::Pose &current_pose);
+geometry_msgs::Point transformToRelativeCoordinate3D(const geometry_msgs::Point &point,
+                                                                      const geometry_msgs::Pose &current_pose);
+
+#endif  // LIBWAYPOINT_FOLLOWER_LIBWAYPOINT_FOLLOWER_H

src/ros/catkin/src/libwaypoint_follower/include/libwaypoint_follower/pure_pursuit.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright 2015-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.
+ */
+
+#ifndef LIBWAYPOINT_FOLLOWER_PURE_PURSUIT_H
+#define LIBWAYPOINT_FOLLOWER_PURE_PURSUIT_H
+
+#define EIGEN_MPL2_ONLY
+
+// ROS includes
+#include <geometry_msgs/Pose.h>
+
+// C++ includes
+#include <memory>
+#include <utility>
+#include <vector>
+
+class PurePursuit
+{
+public:
+  PurePursuit() : use_lerp_(false), lookahead_distance_(0.0), clst_thr_dist_(3.0), clst_thr_ang_(M_PI/4) {}
+
+  // setter
+  void setUseLerp(bool ul);
+  void setCurrentPose(const geometry_msgs::Pose &msg);
+  void setWaypoints(const std::vector<geometry_msgs::Pose> &msg);
+  void setLookaheadDistance(double ld);
+  void setClosestThreshold(double clst_thr_dist, double clst_thr_ang);
+
+  // getter
+  geometry_msgs::Point getLocationOfNextWaypoint();
+  geometry_msgs::Point getLocationOfNextTarget();
+
+  bool isRequirementsSatisfied();
+  std::pair<bool, double> run();  // calculate curvature
+
+private:
+  // variables for debug
+  geometry_msgs::Point loc_next_wp_;
+  geometry_msgs::Point loc_next_tgt_;
+
+  // variables got from outside
+  bool use_lerp_;
+  double lookahead_distance_, clst_thr_dist_, clst_thr_ang_;
+  std::shared_ptr<std::vector<geometry_msgs::Pose>> curr_wps_ptr_;
+  std::shared_ptr<geometry_msgs::Pose> curr_pose_ptr_;
+
+  // functions
+  int32_t findNextPointIdx(int32_t search_start_idx);
+  std::pair<bool, geometry_msgs::Point> lerpNextTarget(int32_t next_wp_idx);
+};
+
+#endif  // LIBWAYPOINT_FOLLOWER_PURE_PURSUIT_H

src/ros/catkin/src/libwaypoint_follower/include/libwaypoint_follower/test_libwaypoint_follower.h

@@ -0,0 +1,94 @@
+/*
+ * Copyright 2018-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.
+ */
+
+#ifndef LIBWAYPOINT_FOLLOWER_TEST_LIBWAYPOINT_FOLLOWER_H
+#define LIBWAYPOINT_FOLLOWER_TEST_LIBWAYPOINT_FOLLOWER_H
+
+#include <gtest/gtest.h>
+#include "libwaypoint_follower/libwaypoint_follower.h"
+
+enum class CoordinateResult
+{
+  Positive = 1,
+  Negative = -1,
+  Equal = 0
+};
+
+struct DirectionCheckDataSet
+{
+  int idx;
+  double vel;
+  DirectionCheckDataSet(int i, double v) :
+    idx(i), vel(v)
+  {}
+  DirectionCheckDataSet() {}
+};
+
+struct ClosestCheckDataSet
+{
+  int dir;
+  double vel;
+  double offset;
+  int num;
+  geometry_msgs::PoseStamped pose;
+  ClosestCheckDataSet(int d, double v, double o, int n, const geometry_msgs::PoseStamped& p)
+    : dir(d), vel(v), offset(o), num(n), pose(p) {}
+  ClosestCheckDataSet() {}
+};
+
+class LibWaypointFollowerTestClass
+{
+public:
+  LibWaypointFollowerTestClass() {}
+  autoware_msgs::Lane generateLane(int driving_direction, double velocity)
+  {
+    return std::move(generateOffsetLane(driving_direction, velocity, 0.0, 100));
+  }
+
+  autoware_msgs::Lane generateOffsetLane(int driving_direction, double velocity, double offset, int num)
+  {
+    autoware_msgs::Lane lane;
+    for (int idx = 0; idx < num; idx++)
+    {
+      static autoware_msgs::Waypoint wp;
+      wp.gid = idx;
+      wp.lid = idx;
+      wp.pose.pose.position.x = driving_direction * (static_cast<double>(idx) + offset);
+      wp.pose.pose.position.y = 0.0;
+      wp.pose.pose.position.z = 0.0;
+      wp.twist.twist.linear.x = velocity;
+      wp.twist.twist.angular.z = 0.0;
+
+      tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, 0);
+      quaternionTFToMsg(quaternion, wp.pose.pose.orientation);
+
+      lane.waypoints.emplace_back(wp);
+    }
+    return std::move(lane);
+  }
+
+  geometry_msgs::PoseStamped generateCurrentPose(double x, double y, double yaw)
+  {
+    geometry_msgs::PoseStamped pose;
+    pose.pose.position.x = x;
+    pose.pose.position.y = y;
+    tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, yaw);
+    quaternionTFToMsg(quaternion, pose.pose.orientation);
+    return std::move(pose);
+  }
+};
+
+#endif  // LIBWAYPOINT_FOLLOWER_TEST_LIBWAYPOINT_FOLLOWER_H

src/ros/catkin/src/libwaypoint_follower/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>libwaypoint_follower</name>
+  <version>1.12.0</version>
+  <description>The libwaypoint_follower package</description>
+  <maintainer email="h_ohta@ertl.jp">h_ohta</maintainer>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <test_depend>rostest</test_depend>
+
+  <depend>amathutils_lib</depend>
+  <depend>autoware_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>roslint</depend>
+  <depend>rosunit</depend>
+  <depend>std_msgs</depend>
+  <depend>eigen</depend>
+</package>