support multiple hw ids and updater ids per updater

README.md

@@ -42,6 +42,38 @@ In a typical architecture indicated above, you will need to build the packages
 [micro_ros_diagnostic_msgs](./micro_ros_diagnostic_msgs/) and
 [micro_ros_diagnostic_bridge](./micro_ros_diagnostic_bridge/) on the micro processor next to the micro-ROS agent.
 
+## Running the example ##
+
+The examples can be used as a usage reference. You'll find an [example launch configuration](micro_ros_diagnostic_bridge/example/launch/example_diagnostic_bridge.launch.py) on the micro_ros_diagnostic_bridge. This example can be run along the examples available in the [micro_ros_diagnostic_updater package](micro_ros_diagnostic_updater).
+
+First, build both packages with the examples enabled
+```
+colcon build --cmake-args -DMICRO_ROS_DIAGNOSTIC_BRIDGE_EXAMPLES=ON -DMICRO_ROS_DIAGNOSTIC_UPDATER_EXAMPLES=ON
+```
+
+You'll need three (or four) terminals for the second step. On the first one, source your `setup.sh` and launch the example bridge
+
+```
+. install/setup.sh
+ros2 launch micro_ros_diagnostic_bridge example_diagnostic_bridge.launch.py
+```
+
+On the second terminal, source `setup.sh` and run the [processor info example](../micro_ros_diagnostic_updater/example/example_processor_updater.c)
+
+```
+. install/setup.sh
+ros2 run micro_ros_diagnostic_updater example_processor_updater
+```
+
+Lastly, on the third terminal, see the diagnostic messages
+
+```
+. install/setup.sh
+ros2 topic echo /diagnostics
+```
+
+You can open a fourth terminal and run the [website checker example](../micro_ros_diagnostic_updater/example/example_website_checker.c)
+
 ## License
 
 The micro-ROS diagnostics framework packages are open-sourced under the Apache-2.0 license. See the [LICENSE](LICENSE) file for details.

micro_ros_common_diagnostics/src/hwmonitor.c

@@ -85,13 +85,13 @@ int main(int argc, const char * argv[])
 
   // updater
   diagnostic_updater_t updater;
-  rc = rclc_diagnostic_updater_init(&updater, &updater_node, hardware_id, updater_id);
+  rc = rclc_diagnostic_updater_init(&updater, &updater_node);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic updater\n");
     return -1;
   }
   diagnostic_task_t task;
-  rc = rclc_diagnostic_task_init(&task, task_id, &my_diagnostic_task);
+  rc = rclc_diagnostic_task_init(&task, hardware_id, updater_id, task_id, &my_diagnostic_task);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic task\n");
     return -1;
@@ -107,7 +107,6 @@ int main(int argc, const char * argv[])
   }
 
   for (unsigned int i = 0; i < 100; ++i) {
-    printf("Publishing processor diagnostics\n");
     rc = rclc_diagnostic_updater_update(&updater);
     if (rc != RCL_RET_OK) {
       printf("Error in publishing processor diagnostics\n");

micro_ros_diagnostic_bridge/CMakeLists.txt

@@ -15,6 +15,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+# build options
+option(MICRO_ROS_DIAGNOSTIC_BRIDGE_EXAMPLES "Build example updaters for the micro-ROS diagnostic bridge package." FALSE)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
@@ -50,6 +53,13 @@ install(DIRECTORY include/ DESTINATION include)
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
+if(MICRO_ROS_DIAGNOSTIC_BRIDGE_EXAMPLES)
+  # launch
+  install(DIRECTORY example/launch DESTINATION share/${PROJECT_NAME}/)
+  # table
+  install(FILES example/example_table.yaml DESTINATION share/${PROJECT_NAME}/)
+endif()
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)

micro_ros_diagnostic_bridge/README.md

@@ -4,7 +4,7 @@ General information about this repository, including legal information, build in
 
 micro-ROS diagnostic bridge is a [ROS 2](http://www.ros2.org/) package that provides a bridge to translate micro-ROS diagnostic messages to vanilla ROS 2 diagnostic messages based on a lookup table.
 
-An exemplary lookup table can be found in: [example_table.yaml](example_table.yaml)
+An exemplary lookup table can be found in: [example_table.yaml](example/example_table.yaml), along with an [example launch file](example/launch/example_diagnostic_bridge.launch.py)
 
 ## Purpose of the Project
 
@@ -18,7 +18,17 @@ standards, e.g., ISO 26262.
 ## How to Build, Test, Install, and Use
 
 After you cloned this repository into your ROS 2 workspace folder, you may build and install it using colcon:
-$ `colcon build --packages-select micro_ros_diagnostic_bridge`
+```
+colcon build --packages-select micro_ros_diagnostic_bridge
+```
+
+### Build with examples ###
+
+As mentioned, this package does not build the examples by default, to do so, you can build with
+
+```
+colcon build --packages-select micro_ros_diagnostic_bridge --cmake-args -DMICRO_ROS_DIAGNOSTIC_BRIDGE_EXAMPLES=ON
+```
 
 ## License
 

micro_ros_diagnostic_bridge/example_table.yaml → micro_ros_diagnostic_bridge/example/example_table.yaml

@@ -3,17 +3,17 @@
 
 hardware_ids:
   ros__parameters:
-    00: esp32_01
-    17: esp32_foo
+    1001: esp32_01
+    998: esp32_foo
     42: esp32_bar
 
 updaters:
   ros__parameters:
-    00:
+    0:
       name: "google.com checker"
       description: "Periodically checks the website google.com for availability."
       keys:
-        23:
+        42:
           name: "return code"
           values:
             200: "ok"
@@ -23,7 +23,7 @@ updaters:
       name: "Processor info"
       description: "Measuring processor temperature and load."
       keys:
-        00:
+        0:
           name: "temp"
-        01:
+        1:
           name: "load"

micro_ros_diagnostic_bridge/example/launch/example_diagnostic_bridge.launch.py

@@ -0,0 +1,46 @@
+# Copyright (c) 2021 - for information on the respective copyright owner
+# see the NOTICE file and/or the repository https://github.com/micro-ROS/system_modes.
+#
+# 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.
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python import get_package_share_directory
+import os
+
+logger = LaunchConfiguration('log_level')
+
+
+def generate_launch_description():
+
+    bridge_launch = os.path.join(get_package_share_directory(
+        "micro_ros_diagnostic_bridge"), "launch/diagnostic_bridge.launch.py")
+
+    lookup_table = os.path.join(get_package_share_directory(
+        "micro_ros_diagnostic_bridge"), "example_table.yaml")
+
+    example_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            bridge_launch),
+        launch_arguments={
+            'lookup_table': lookup_table
+        }.items()
+    )
+
+    return LaunchDescription([
+        example_bridge
+    ])

micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp

@@ -45,8 +45,8 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
     {
       RCLCPP_DEBUG(
         get_logger(),
-        "Bridging message from hardware %d, updater %d",
-        msg_in->hardware_id, msg_in->updater_id);
+        "Bridging message from hardware %d, updater %d", msg_in->hardware_id,
+        msg_in->updater_id);
       msg_out_ = std::make_unique<diagnostic_msgs::msg::DiagnosticStatus>();
 
       auto updater = lookup_updater(msg_in->updater_id);
@@ -57,6 +57,7 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
       msg_out_->level = msg_in->level;
 
       diagnostic_msgs::msg::KeyValue keyvalue;
+      RCLCPP_DEBUG(get_logger(), "Bridging updater %d, task %d", msg_in->updater_id, msg_in->key);
       keyvalue.key = lookup_key(msg_in->updater_id, msg_in->key);
       switch (msg_in->value_type) {
         case micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus::VALUE_BOOL:
@@ -77,13 +78,16 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
       ros2_pub_->publish(std::move(msg_out_));
     };
 
+  rclcpp::QoS qos{rclcpp::KeepLast{10}};
+  qos.reliable();
+
   uros_sub_ = create_subscription<MicroROSDiagnosticStatus>(
     UROS_DIAGNOSTICS_BRIDGE_TOPIC_IN,
-    rclcpp::SystemDefaultsQoS(),
+    qos,
     callback);
   ros2_pub_ = create_publisher<DiagnosticStatus>(
     UROS_DIAGNOSTICS_BRIDGE_TOPIC_OUT,
-    rclcpp::SystemDefaultsQoS());
+    qos);
 }
 
 std::string
@@ -163,16 +167,23 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
   for (it = param_map.begin(); it != param_map.end(); it++) {
     if (it->first.compare("/hardware_ids") == 0) {
       for (auto & p : it->second) {
-        hardware_map_[std::stoi(p.get_name())] = p.value_to_string();
+        try {
+          hardware_map_[std::stoi(p.get_name())] = p.value_to_string();
+        } catch (const std::invalid_argument &) {
+          throw std::runtime_error("Failed to parse hardware_id from lookup_table.");
+        }
       }
     }
 
     if (it->first.compare("/updaters") == 0) {
       std::string updater_key, updater_name, updater_descr, key, key_name;
       for (auto & p : it->second) {
+
         auto pos = p.get_name().find('.');
         if (pos != std::string::npos) {
           updater_key = p.get_name().substr(0, pos);
+        } else {
+          throw std::runtime_error("Failed to load updater key.");
         }
 
         // Updater

micro_ros_diagnostic_updater/CMakeLists.txt

@@ -50,19 +50,31 @@ include_directories(include)
 if(MICRO_ROS_DIAGNOSTIC_UPDATER_EXAMPLES)
   message(STATUS "Building example updaters")
 
-  add_executable(example_processor_updater example/example_processor_updater.c)
-  target_link_libraries(example_processor_updater ${PROJECT_NAME})
-  ament_target_dependencies(example_processor_updater
+  set(processor_example example_processor_updater)
+  add_executable(${processor_example} example/example_processor_updater.c)
+  target_link_libraries(${processor_example} ${PROJECT_NAME})
+  ament_target_dependencies(${processor_example}
     rclc
     rcutils
     micro_ros_diagnostic_msgs)
 
-  add_executable(example_website_checker example/example_website_checker.c)
-  target_link_libraries(example_website_checker ${PROJECT_NAME})
-  ament_target_dependencies(example_website_checker
+  install(TARGETS
+    ${processor_example}
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+
+  set(website_example example_website_checker)
+  add_executable(${website_example} example/example_website_checker.c)
+  target_link_libraries(${website_example} ${PROJECT_NAME})
+  ament_target_dependencies(${website_example}
     rclc
     rcutils
     micro_ros_diagnostic_msgs)
+
+  install(TARGETS
+    ${website_example}
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
 endif()
 
 # install
@@ -101,5 +113,6 @@ ament_export_dependencies(ament_cmake)
 ament_export_dependencies(micro_ros_diagnostic_msgs)
 ament_export_dependencies(rosidl_generator_c)
 ament_export_dependencies(rcl)
+ament_export_dependencies(rclc)
 ament_export_dependencies(rcutils)
 ament_package()

micro_ros_diagnostic_updater/README.md

@@ -4,7 +4,9 @@ General information about this repository, including legal information, build in
 
 micro-ROS diagnostic updater is a [ROS 2](http://www.ros2.org/) package that provides convenience functions to implement diagnostic tasks amd updaters based on the ROS Client C-Library (RCLC) for micro-ROS.
 
-An exemplary implementation can be found in: [example/example_updater.c](example_updater.c)
+An exemplary implementation can be found in: [example/example_updater.c](example_updater.c).
+
+The examples are disabled by default, to build the examples, build with `MICRO_ROS_DIAGNOSTIC_UPDATER_EXAMPLES=ON`
 
 ## Purpose of the Project
 
@@ -18,7 +20,16 @@ standards, e.g., ISO 26262.
 ## How to Build, Test, Install, and Use
 
 After you cloned this repository into your ROS 2 workspace folder, you may build and install it using colcon:
-$ `colcon build --packages-select micro_ros_diagnostic_updater`
+```
+colcon build --packages-select micro_ros_diagnostic_updater
+```
+### Build with examples ###
+
+As mentioned, this package does not build the examples by default, to do so, you can build with
+
+```
+colcon build --packages-select micro_ros_diagnostic_updater --cmake-args -DMICRO_ROS_DIAGNOSTIC_UPDATER_EXAMPLES=ON
+```
 
 ## License
 

micro_ros_diagnostic_updater/example/example_processor_updater.c

@@ -19,7 +19,15 @@
 
 #include "micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h"
 
-static int my_diagnostic_temp = 0;
+static uint32_t my_diagnostic_temp = 0;
+// The updater id
+static const int16_t PROCESSOR_ID = 17;
+// The hardware id
+static const int16_t PROCESSOR_SERIAL = 1001;
+// Task id
+static const int16_t PROCESSOR_TEMPERATURE_TASK_ID = 0;
+// Task id
+static const int16_t PROCESSOR_LOAD_TASK_ID = 1;
 
 rcl_ret_t
 my_diagnostic_temperature(diagnostic_value_t * temp_kv)
@@ -94,22 +102,22 @@ int main(int argc, const char * argv[])
 
   // updater
   diagnostic_updater_t updater;
-  rc = rclc_diagnostic_updater_init(&updater, &my_node, 17, 42);
+  rc = rclc_diagnostic_updater_init(&updater, &my_node);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic updater\n");
     return -1;
   }
-  diagnostic_task_t task;
+  diagnostic_task_t temperature_task;
   rc = rclc_diagnostic_task_init(
-    &task, 0,
+    &temperature_task, PROCESSOR_SERIAL, PROCESSOR_ID, PROCESSOR_TEMPERATURE_TASK_ID,
     &my_diagnostic_temperature);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic task\n");
     return -1;
   }
-  diagnostic_task_t ltask;
+  diagnostic_task_t load_task;
   rc = rclc_diagnostic_task_init(
-    &ltask, 1,
+    &load_task, PROCESSOR_SERIAL, PROCESSOR_ID, PROCESSOR_LOAD_TASK_ID,
     &my_diagnostic_load);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic website checker\n");
@@ -119,21 +127,20 @@ int main(int argc, const char * argv[])
   // adding tasks
   rc = rclc_diagnostic_updater_add_task(
     &updater,
-    &task);
+    &temperature_task);
   if (rc != RCL_RET_OK) {
     printf("Error in adding diagnostic temp task\n");
     return -1;
   }
   rc = rclc_diagnostic_updater_add_task(
     &updater,
-    &ltask);
+    &load_task);
   if (rc != RCL_RET_OK) {
     printf("Error in adding diagnostic website task\n");
     return -1;
   }
 
   for (unsigned int i = 0; i < 100; ++i) {
-    printf("Publishing processor diagnostics\n");
     rc = rclc_diagnostic_updater_update(&updater);
     if (rc != RCL_RET_OK) {
       printf("Error in publishing temp diagnostics\n");