Merge branch 'feature/add-analyzers-through-parameters' into nobleo-ros2

diagnostic_aggregator/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
@@ -67,6 +68,10 @@ add_executable(aggregator_node src/aggregator_node.cpp)
 target_link_libraries(aggregator_node
   ${PROJECT_NAME})
 
+# Add analyzer
+add_executable(add_analyzer src/add_analyzer.cpp)
+ament_target_dependencies(add_analyzer rclcpp rcl_interfaces)
+
 # Testing macro
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -135,6 +140,11 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  TARGETS add_analyzer
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(
   TARGETS ${PROJECT_NAME} ${ANALYZERS}
   EXPORT ${PROJECT_NAME}Targets

diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp

@@ -133,6 +133,8 @@ class Aggregator
   rclcpp::Service<diagnostic_msgs::srv::AddDiagnostics>::SharedPtr add_srv_;
   /// DiagnosticArray, /diagnostics
   rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_sub_;
+  /// ParameterEvent, /parameter_events
+  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr param_sub_;
   /// DiagnosticArray, /diagnostics_agg
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr agg_pub_;
   /// DiagnosticStatus, /diagnostics_toplevel_state
@@ -165,6 +167,16 @@ class Aggregator
   /// Records all ROS warnings. No warnings are repeated.
   std::set<std::string> ros_warnings_;
 
+  /*
+   *!\brief Checks for new parameters to trigger reinitialization of the AnalyzerGroup and OtherAnalyzer
+   */
+  void parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr param_msg);
+
+  /*
+   *!\brief (re)initializes the AnalyzerGroup and OtherAnalyzer
+   */
+  void initAnalyzers();
+
   /*
    *!\brief Checks timestamp of message, and warns if timestamp is 0 (not set)
    */

diagnostic_aggregator/package.xml

@@ -12,7 +12,7 @@
   <license>BSD-3-Clause</license>
 
   <url type="website">http://www.ros.org/wiki/diagnostic_aggregator</url>
-  
+
   <author>Kevin Watts</author>
   <author email="[email protected]">Brice Rebsamen</author>
   <author email="[email protected]">Arne Nordmann</author>
@@ -22,6 +22,7 @@
 
   <build_depend>diagnostic_msgs</build_depend>
   <build_depend>pluginlib</build_depend>
+  <build_depend>rcl_interfaces</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>std_msgs</build_depend>
 

diagnostic_aggregator/src/add_analyzer.cpp

@@ -0,0 +1,100 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**< \author Martin Cornelis */
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rcl_interfaces/msg/parameter.hpp"
+
+using namespace std::chrono_literals;
+
+class AddAnalyzer : public rclcpp::Node
+{
+public:
+  AddAnalyzer()
+  : Node("add_analyzer_node", "", rclcpp::NodeOptions().allow_undeclared_parameters(
+        true).automatically_declare_parameters_from_overrides(true))
+  {
+    client_ = this->create_client<rcl_interfaces::srv::SetParametersAtomically>(
+      "/diagnostics_agg/set_parameters_atomically");
+  }
+
+  void send_request()
+  {
+    while (!client_->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+        return;
+      }
+      RCLCPP_INFO_ONCE(this->get_logger(), "service not available, waiting ...");
+    }
+    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
+    std::map<std::string, rclcpp::Parameter> parameters;
+    if (this->get_parameters("", parameters)) {
+      for (const auto & param : parameters) {
+        if (param.first.substr(0, analyzers_ns_.length()).compare(analyzers_ns_) == 0) {
+          auto parameter_msg = param.second.to_parameter_msg();
+          request->parameters.push_back(parameter_msg);
+        }
+      }
+    }
+    auto result = client_->async_send_request(request);
+    // Wait for the result.
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_INFO(this->get_logger(), "Parameters succesfully set");
+    } else {
+      RCLCPP_ERROR(this->get_logger(), "Failed to set parameters");
+    }
+  }
+
+private:
+  rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr client_;
+  std::string analyzers_ns_ = "analyzers.";
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto add_analyzer = std::make_shared<AddAnalyzer>();
+  add_analyzer->send_request();
+  rclcpp::shutdown();
+
+  return 0;
+}

diagnostic_aggregator/src/aggregator.cpp

@@ -59,7 +59,8 @@ using diagnostic_msgs::msg::DiagnosticStatus;
 Aggregator::Aggregator()
 : n_(std::make_shared<rclcpp::Node>(
       "analyzers", "",
-      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))),
+      rclcpp::NodeOptions().allow_undeclared_parameters(true).
+      automatically_declare_parameters_from_overrides(true))),
   logger_(rclcpp::get_logger("Aggregator")),
   pub_rate_(1.0),
   history_depth_(1000),
@@ -69,6 +70,35 @@ Aggregator::Aggregator()
   last_top_level_state_(DiagnosticStatus::STALE)
 {
   RCLCPP_DEBUG(logger_, "constructor");
+  initAnalyzers();
+
+  diag_sub_ = n_->create_subscription<DiagnosticArray>(
+    "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_),
+    std::bind(&Aggregator::diagCallback, this, _1));
+  agg_pub_ = n_->create_publisher<DiagnosticArray>("/diagnostics_agg", 1);
+  toplevel_state_pub_ =
+    n_->create_publisher<DiagnosticStatus>("/diagnostics_toplevel_state", 1);
+
+  int publish_rate_ms = 1000 / pub_rate_;
+  publish_timer_ = n_->create_wall_timer(
+    std::chrono::milliseconds(publish_rate_ms),
+    std::bind(&Aggregator::publishData, this));
+
+  param_sub_ = n_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
+    "/parameter_events", 1, std::bind(&Aggregator::parameterCallback, this, _1));
+}
+
+void Aggregator::parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg)
+{
+  if (msg->node == "/" + std::string(n_->get_name())) {
+    if (msg->new_parameters.size() != 0) {
+      initAnalyzers();
+    }
+  }
+}
+
+void Aggregator::initAnalyzers()
+{
   bool other_as_errors = false;
 
   std::map<std::string, rclcpp::Parameter> parameters;
@@ -101,26 +131,17 @@ Aggregator::Aggregator()
   RCLCPP_DEBUG(
     logger_, "Aggregator critical publisher configured to: %s", (critical_ ? "true" : "false"));
 
-  analyzer_group_ = std::make_unique<AnalyzerGroup>();
-  if (!analyzer_group_->init(base_path_, "", n_)) {
-    RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!");
-  }
-
-  // Last analyzer handles remaining data
-  other_analyzer_ = std::make_unique<OtherAnalyzer>(other_as_errors);
-  other_analyzer_->init(base_path_);  // This always returns true
-
-  diag_sub_ = n_->create_subscription<DiagnosticArray>(
-    "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_),
-    std::bind(&Aggregator::diagCallback, this, _1));
-  agg_pub_ = n_->create_publisher<DiagnosticArray>("/diagnostics_agg", 1);
-  toplevel_state_pub_ =
-    n_->create_publisher<DiagnosticStatus>("/diagnostics_toplevel_state", 1);
+  {  // lock the mutex while analyzer_group_ and other_analyzer_ are being updated
+    std::lock_guard<std::mutex> lock(mutex_);
+    analyzer_group_ = std::make_unique<AnalyzerGroup>();
+    if (!analyzer_group_->init(base_path_, "", n_)) {
+      RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!");
+    }
 
-  int publish_rate_ms = 1000 / pub_rate_;
-  publish_timer_ = n_->create_wall_timer(
-    std::chrono::milliseconds(publish_rate_ms),
-    std::bind(&Aggregator::publishData, this));
+    // Last analyzer handles remaining data
+    other_analyzer_ = std::make_unique<OtherAnalyzer>(other_as_errors);
+    other_analyzer_->init(base_path_);  // This always returns true
+  }
 }
 
 void Aggregator::checkTimestamp(const DiagnosticArray::SharedPtr diag_msg)