Dev ros2 diagnostic (#149)

* load separate diagnostic yaml for each machine model

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

* add ace_diagnostic.yaml

Signed-off-by: yamahatayukiya <y2yamahata@jst.go.jp>

* adjust ros time to match bag playback

Signed-off-by: yamahatayukiya <y2yamahata@jst.go.jp>

* add environment variable to set people fps

Signed-off-by: Tatsuya Ishihara <tisihara@jp.ibm.com>

* add CabotAnalyzer diagnostic template

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* create the content of cabot_analyzer

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

* modify to use generic_analyzer

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

* add CopyRight and rename member variable

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

* Make adjustments related to the configuration

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

* Lower PeopleTrack hz minimum

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>

---------

Signed-off-by: Fuki Miyazaki <f2miyazaki@lab.miraikan.jst.go.jp>
Signed-off-by: yamahatayukiya <y2yamahata@jst.go.jp>
Signed-off-by: Tatsuya Ishihara <tisihara@jp.ibm.com>
Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
Co-authored-by: yamahatayukiya <y2yamahata@jst.go.jp>
Co-authored-by: Tatsuya Ishihara <tisihara@jp.ibm.com>
Co-authored-by: Daisuke Sato <daisukes@cmu.edu>

cabot_common/scripts/footprint_publisher.py

@@ -51,9 +51,6 @@ def check_status(stat):
     if not footprint:
         stat.summary(DiagnosticStatus.WARN, "No footprint (mode={})".format(current_mode))
         return stat
-    if len(transforms) == 0:
-        stat.summary(DiagnosticStatus.WARN, "No transform is specified (mode={})".format(current_mode))
-        return stat
     stat.summary(DiagnosticStatus.OK, "working (mode={})".format(current_mode))
     return stat
 
@@ -165,7 +162,6 @@ def get_offset_joint_state(mode):
 
 current_mode = None
 footprint = None
-transforms = []
 
 
 def timer_callback():

cabot_diagnostics/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.8)
+project(cabot_diagnostics)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wfatal-errors)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(diagnostic_aggregator REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(include)
+
+add_library(cabot_diagnostics_analyzers SHARED
+  src/cabot_analyzer.cpp)
+
+ament_target_dependencies(
+  cabot_diagnostics_analyzers
+  diagnostic_aggregator
+  pluginlib
+  diagnostic_msgs
+  rclcpp
+)
+
+install(TARGETS
+  cabot_diagnostics_analyzers
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+target_include_directories(cabot_diagnostics_analyzers PRIVATE
+  ${diagnostic_aggregator_INCLUDE_DIRS}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+pluginlib_export_plugin_description_file(diagnostic_aggregator plugin_description.xml)
+
+ament_package()

cabot_diagnostics/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cabot_diagnostics</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="f2miyazaki@lab.miraikan.jst.go.jp">cabot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake_python</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
+  <depend>pluginlib</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_aggregator</depend>
+
+
+  <!-- <exec_depend>diagnostic_aggregator</exec_depend> -->
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

cabot_diagnostics/plugin_description.xml

@@ -0,0 +1,8 @@
+<library path="cabot_diagnostics_analyzers" >
+  <class name="cabot_diagnostics/CabotAnalyzer" type="cabot_diagnostics::CabotAnalyzer" base_class_type="diagnostic_aggregator::Analyzer">
+    <description>
+      CabotAnalyzer is custom diagnostic analyzer.
+    </description>
+  </class>
+</library>
+

cabot_diagnostics/src/cabot_analyzer.cpp

@@ -0,0 +1,112 @@
+/*******************************************************************************
+ * Copyright (c) 2023  Miraikan and Carnegie Mellon University
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *******************************************************************************/
+
+#include "diagnostic_aggregator/generic_analyzer.hpp"
+#include "cabot_analyzer.hpp"
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/parameter.hpp"
+
+PLUGINLIB_EXPORT_CLASS(cabot_diagnostics::CabotAnalyzer, diagnostic_aggregator::Analyzer)
+
+namespace cabot_diagnostics
+{
+using std::string;
+using std::vector;
+
+CabotAnalyzer::CabotAnalyzer() {}
+
+bool CabotAnalyzer::init(
+  const std::string & path, const std::string & breadcrumb, const rclcpp::Node::SharedPtr node)
+{
+  treat_as_warning_ = false;
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("CabotAnalyzer"), "CabotAnalyzer, breadcrumb: %s", breadcrumb.c_str());
+
+  std::map<std::string, rclcpp::Parameter> parameters;
+  if (!node->get_parameters(breadcrumb, parameters)) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("CabotAnalyzer"),
+      "Couldn't retrieve parameters for generic analyzer at prefix '%s'.", breadcrumb.c_str());
+    return false;
+  }
+
+  for (const auto & param : parameters) {
+    string pname = param.first;
+    rclcpp::Parameter pvalue = param.second;
+    if (pname.compare("treat_as_warning") == 0){
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("CabotAnalyzer"), "CabotAnalyzer '%s' found treat_as_warning: %s",
+        getName().c_str(), pvalue.value_to_string().c_str());
+      treat_as_warning_ = true;
+    }
+  }
+
+  return analyzer_.init(path, breadcrumb, node);
+}
+
+CabotAnalyzer::~CabotAnalyzer() {}
+
+bool CabotAnalyzer::match(const string & name)
+{
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("CabotAnalyzer"), "CabotAnalyzer '%s' match %s", getName().c_str(),
+    name.c_str());
+
+  return analyzer_.match(name);
+}
+
+bool CabotAnalyzer::analyze(const std::shared_ptr<diagnostic_aggregator::StatusItem> item) {
+  RCLCPP_DEBUG(
+      rclcpp::get_logger("CabotAnalyzer"), "CabotAnalyzer '%s' analyze, item %s: %s",
+      getName().c_str(), item->getName().c_str(), item->getMessage().c_str());
+
+  return analyzer_.analyze(item);
+}
+
+vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> CabotAnalyzer::report()
+{
+  RCLCPP_DEBUG(
+      rclcpp::get_logger("CabotAnalyzer"), "CabotAnalyzer '%s' report()", getName().c_str());
+
+  std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> processed = analyzer_.report();
+
+  if (treat_as_warning_ && processed[0]->level > diagnostic_msgs::msg::DiagnosticStatus::WARN) {
+    processed[0]->level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  }
+
+  return processed;
+}
+
+std::string CabotAnalyzer::getPath() const {
+  return analyzer_.getPath();
+}
+
+std::string CabotAnalyzer::getName() const {
+  return analyzer_.getName();
+}
+
+}  // namespace cabot_diagnostics

cabot_diagnostics/src/cabot_analyzer.hpp

@@ -0,0 +1,87 @@
+/*******************************************************************************
+ * Copyright (c) 2023  Miraikan and Carnegie Mellon University
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *******************************************************************************/
+
+#ifndef DIAGNOSTIC_AGGREGATOR__CABOT_ANALYZER_HPP_
+#define DIAGNOSTIC_AGGREGATOR__CABOT_ANALYZER_HPP_
+
+#include <map>
+#include <memory>
+#include <regex>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "diagnostic_aggregator/analyzer.hpp"
+#include "diagnostic_aggregator/generic_analyzer_base.hpp"
+#include "diagnostic_aggregator/status_item.hpp"
+#include "diagnostic_aggregator/visibility_control.hpp"
+
+#include "diagnostic_msgs/msg/diagnostic_status.h"
+#include "diagnostic_msgs/msg/key_value.h"
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace cabot_diagnostics
+{
+class CabotAnalyzer : public diagnostic_aggregator::Analyzer
+{
+public:
+  /*!
+   *\brief Default constructor loaded by pluginlib
+   */
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  CabotAnalyzer();
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  virtual ~CabotAnalyzer();
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  bool init(
+      const std::string & base_path, const std::string & breadcrumb,
+      const rclcpp::Node::SharedPtr node);
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  bool match(const std::string & name);
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  bool analyze(const std::shared_ptr<diagnostic_aggregator::StatusItem> item);
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  std::vector<std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus>> report();
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  std::string getPath() const;
+
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  std::string getName() const;
+
+private:
+  diagnostic_aggregator::GenericAnalyzer analyzer_;
+  bool treat_as_warning_;
+};
+
+}  // namespace cabot_diagnostics
+
+#endif  // DIAGNOSTIC_AGGREGATOR__GENERIC_ANALYZER_HPP_