Use unsigned diagnostic translation indexes

NOTICE

@@ -24,5 +24,7 @@
 Capra Robotics ApS
     Bartolome Jimenez Vera <bjv@capra.ooo>
 
+Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com>
+
 Robert Bosch GmbH
     Arne Nordmann <arne.nordmann@de.bosch.com>

micro_ros_diagnostic_bridge/include/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp

@@ -14,7 +14,7 @@
 // limitations under the License.
 #pragma once
 
-#include <climits>
+#include <cstdint>
 #include <map>
 #include <memory>
 #include <string>
@@ -33,8 +33,6 @@ static const char UROS_DIAGNOSTICS_BRIDGE_TOPIC_OUT[] = "diagnostics";
 namespace uros_diagnostic_msg = micro_ros_diagnostic_msgs::msg;
 namespace diagnostic_msg = diagnostic_msgs::msg;
 
-constexpr int UNIQUE_POLYNOM = 4567;
-
 struct MicroROSDiagnosticUpdater
 {
   std::string name;
@@ -43,31 +41,29 @@ struct MicroROSDiagnosticUpdater
 
 struct MicroROSDiagnosticKey
 {
-  int updater_id;
-  int key_id;
+  uint16_t updater_id;
+  uint16_t key_id;
 
   bool operator<(const MicroROSDiagnosticKey & rhs) const
   {
-    return (updater_id * UNIQUE_POLYNOM + key_id) < (rhs.updater_id * UNIQUE_POLYNOM + rhs.key_id);
+    return std::tie(updater_id, key_id) < std::tie(rhs.updater_id, rhs.key_id);
   }
 };
 
 struct MicroROSDiagnosticValue
 {
   MicroROSDiagnosticKey task;
-  int value_id;
+  uint16_t value_id;
 
   bool operator<(const MicroROSDiagnosticValue & rhs) const
   {
-    return (task.updater_id * UNIQUE_POLYNOM * UNIQUE_POLYNOM + task.key_id * UNIQUE_POLYNOM +
-           value_id) <
-           (rhs.task.updater_id * UNIQUE_POLYNOM * UNIQUE_POLYNOM + rhs.task.key_id *
-           UNIQUE_POLYNOM + rhs.value_id);
+    return std::tie(task.updater_id, task.key_id, value_id) <
+           std::tie(rhs.task.updater_id, rhs.task.key_id, rhs.value_id);
   }
 };
 
-typedef std::map<int, std::string> HardwareMap;
-typedef std::map<int, MicroROSDiagnosticUpdater> UpdaterMap;
+typedef std::map<uint16_t, std::string> HardwareMap;
+typedef std::map<uint16_t, MicroROSDiagnosticUpdater> UpdaterMap;
 typedef std::map<MicroROSDiagnosticKey, std::string> KeyMap;
 typedef std::map<MicroROSDiagnosticValue, std::string> ValueMap;
 
@@ -77,16 +73,16 @@ class MicroROSDiagnosticBridge : public rclcpp::Node
   explicit MicroROSDiagnosticBridge(const std::string & path = "");
 
   std::string lookup_hardware(
-    int hardware_id);
+    uint16_t hardware_id);
   const MicroROSDiagnosticUpdater lookup_updater(
-    int updater_id);
+    uint16_t updater_id);
   std::string lookup_key(
-    int updater_id,
-    int key);
+    uint16_t updater_id,
+    uint16_t key);
   std::string lookup_value(
-    int updater_id,
-    int key,
-    int value_id);
+    uint16_t updater_id,
+    uint16_t key,
+    uint16_t value_id);
 
 private:
   void read_lookup_table(const std::string & path);

micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp

@@ -14,6 +14,7 @@
 // limitations under the License.
 #include "micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp"
 
+#include <limits>
 #include <memory>
 #include <stdexcept>
 #include <string>
@@ -31,6 +32,27 @@ using diagnostic_msgs::msg::DiagnosticArray;
 
 static inline std::string VALUE_NOT_FOUND = "NOTFOUND";
 
+namespace
+{
+uint16_t parse_index(const std::string & text, const std::string & name)
+{
+  try {
+    size_t parsed_length = 0;
+    const unsigned long value = std::stoul(text, &parsed_length);
+    if (text.empty() || text.front() == '-' || parsed_length != text.length() ||
+      value > std::numeric_limits<uint16_t>::max())
+    {
+      throw std::out_of_range("index is outside the uint16 range");
+    }
+    return static_cast<uint16_t>(value);
+  } catch (const std::invalid_argument &) {
+    throw std::runtime_error("Failed to parse " + name + " from lookup_table.");
+  } catch (const std::out_of_range &) {
+    throw std::runtime_error("Failed to parse " + name + " from lookup_table.");
+  }
+}
+}  // namespace
+
 MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
 : Node("micro_ros_diagnostic_bridge"),
   logger_(rclcpp::get_logger("MicroROSDiagnosticBridge"))
@@ -107,8 +129,8 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
 
 std::string
 MicroROSDiagnosticBridge::lookup_key(
-  int updater_id,
-  int key)
+  uint16_t updater_id,
+  uint16_t key)
 {
   try {
     return key_map_.at({updater_id, key});
@@ -123,9 +145,9 @@ MicroROSDiagnosticBridge::lookup_key(
 
 std::string
 MicroROSDiagnosticBridge::lookup_value(
-  int updater_id,
-  int key,
-  int value_id)
+  uint16_t updater_id,
+  uint16_t key,
+  uint16_t value_id)
 {
   try {
     return value_map_.at({{updater_id, key}, value_id});
@@ -139,7 +161,7 @@ MicroROSDiagnosticBridge::lookup_value(
 }
 
 std::string
-MicroROSDiagnosticBridge::lookup_hardware(int hardware_id)
+MicroROSDiagnosticBridge::lookup_hardware(uint16_t hardware_id)
 {
   try {
     return hardware_map_.at(hardware_id);
@@ -153,7 +175,7 @@ MicroROSDiagnosticBridge::lookup_hardware(int hardware_id)
 }
 
 const MicroROSDiagnosticUpdater
-MicroROSDiagnosticBridge::lookup_updater(int updater_id)
+MicroROSDiagnosticBridge::lookup_updater(uint16_t updater_id)
 {
   try {
     return updater_map_.at(updater_id);
@@ -182,14 +204,11 @@ 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) {
-        try {
-          hardware_map_[std::stoi(p.get_name())] = p.value_to_string();
-          RCLCPP_DEBUG(
-            get_logger(), "FOUND Parameter: %s HW_ID %s",
-            p.get_name().c_str(), p.value_to_string().c_str());
-        } catch (const std::invalid_argument &) {
-          throw std::runtime_error("Failed to parse hardware_id from lookup_table.");
-        }
+        const uint16_t hardware_id = parse_index(p.get_name(), "hardware_id");
+        hardware_map_[hardware_id] = p.value_to_string();
+        RCLCPP_DEBUG(
+          get_logger(), "FOUND Parameter: %s HW_ID %s",
+          p.get_name().c_str(), p.value_to_string().c_str());
       }
     }
 
@@ -205,16 +224,17 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
         }
 
         // Updater
+        const uint16_t updater_id = parse_index(updater_key, "updater_id");
         if (p.get_name().compare(updater_key + ".name") == 0) {
           updater_name = p.value_to_string();
           RCLCPP_DEBUG(
             get_logger(), "Updater Name: %s, Description: %s",
             updater_name.c_str(), updater_descr.c_str());
-          updater_map_[std::stoi(updater_key)] = {updater_name, updater_descr};
+          updater_map_[updater_id] = {updater_name, updater_descr};
         }
         if (p.get_name().compare(updater_key + ".description") == 0) {
           updater_descr = p.value_to_string();
-          updater_map_[std::stoi(updater_key)] = {updater_name, updater_descr};
+          updater_map_[updater_id] = {updater_name, updater_descr};
           RCLCPP_DEBUG(
             get_logger(), "Updater Name: %s, Description: %s",
             updater_name.c_str(), updater_descr.c_str());
@@ -230,15 +250,16 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
         if (p.get_name().compare(updater_key + ".keys." + key + ".name") == 0) {
           key_name = p.value_to_string();
           RCLCPP_DEBUG(get_logger(), "Key name: %s", key_name.c_str());
-          key_map_[{std::stoi(updater_key), std::stoi(key)}] = key_name;
+          key_map_[{updater_id, parse_index(key, "key_id")}] = key_name;
         }
 
         // Values lookup
         if (p.get_name().rfind(updater_key + ".keys." + key + ".values") == 0) {
           auto start = updater_key.length() + key.length() + 14;
           pos = p.get_name().find('.', start);
-          auto value_id = std::stoi(p.get_name().substr(start, pos - start));
-          value_map_[{{std::stoi(updater_key), std::stoi(key)}, value_id}] = p.value_to_string();
+          const uint16_t value_id = parse_index(
+            p.get_name().substr(start, pos - start), "value_id");
+          value_map_[{{updater_id, parse_index(key, "key_id")}, value_id}] = p.value_to_string();
           RCLCPP_DEBUG(get_logger(), "Value ID %d Value %s", value_id, p.value_to_string().c_str());
         }
       }

micro_ros_diagnostic_bridge/test/test_diagnostic_bridge.cpp

@@ -17,6 +17,7 @@
 
 #include <gtest/gtest.h>
 #include <rclcpp/rclcpp.hpp>
+#include <limits>
 #include <string>
 
 
@@ -52,11 +53,13 @@ TEST_F(TestDiagnosticBridge, parsing) {
  */
 TEST_F(TestDiagnosticBridge, translating) {
   MicroROSDiagnosticBridge * bridge = new MicroROSDiagnosticBridge(LOOKUP_TABLE_PATH);
+  const uint16_t max_id = std::numeric_limits<uint16_t>::max();
 
   // Hardware
   EXPECT_EQ("esp32_01", bridge->lookup_hardware(0));
   EXPECT_EQ("esp32_foo", bridge->lookup_hardware(17));
   EXPECT_EQ("esp32_bar", bridge->lookup_hardware(42));
+  EXPECT_EQ("esp32_max", bridge->lookup_hardware(max_id));
 
   EXPECT_NO_THROW(bridge->lookup_hardware(23)) << "should be rclcpp error log";
   EXPECT_EQ("NOTFOUND", bridge->lookup_hardware(23));
@@ -70,6 +73,7 @@ TEST_F(TestDiagnosticBridge, translating) {
   EXPECT_EQ(
     "Measuring processor temperature and load.",
     bridge->lookup_updater(17).description);
+  EXPECT_EQ("Maximum IDs", bridge->lookup_updater(max_id).name);
 
   EXPECT_NO_THROW(bridge->lookup_updater(23)) << "should be rclcpp error log";
   EXPECT_EQ("NOTFOUND", bridge->lookup_updater(23).name);
@@ -79,13 +83,15 @@ TEST_F(TestDiagnosticBridge, translating) {
   EXPECT_EQ("return code", bridge->lookup_key(0, 23));
   EXPECT_EQ("temp", bridge->lookup_key(17, 0));
   EXPECT_EQ("load", bridge->lookup_key(17, 1));
+  EXPECT_EQ("max key", bridge->lookup_key(max_id, max_id));
 
   EXPECT_NO_THROW(bridge->lookup_key(17, 23)) << "should be rclcpp error log";
   EXPECT_EQ("NOTFOUND", bridge->lookup_key(17, 23));
   EXPECT_EQ("NOTFOUND", bridge->lookup_key(0, 0));
 
   // Values
   EXPECT_EQ("ok", bridge->lookup_value(0, 23, 200));
+  EXPECT_EQ("max value", bridge->lookup_value(max_id, max_id, max_id));
 
   EXPECT_NO_THROW(bridge->lookup_value(0, 0, 0)) << "should be rclcpp error log";
   EXPECT_EQ("NOTFOUND", bridge->lookup_value(0, 0, 0));

micro_ros_diagnostic_bridge/test/test_lookup_table.yaml

@@ -6,6 +6,7 @@ hardware_ids:
     00: esp32_01
     17: esp32_foo
     42: esp32_bar
+    65535: esp32_max
 
 updaters:
   ros__parameters:
@@ -27,3 +28,11 @@ updaters:
           name: "temp"
         01:
           name: "load"
+    65535:
+      name: "Maximum IDs"
+      description: "Exercises the full uint16 index range."
+      keys:
+        65535:
+          name: "max key"
+          values:
+            65535: "max value"

micro_ros_diagnostic_updater/example/example_processor_updater.c

@@ -21,13 +21,13 @@
 
 static uint32_t my_diagnostic_temp = 0;
 // The updater id
-static const int16_t PROCESSOR_ID = 17;
+static const uint16_t PROCESSOR_ID = 17;
 // The hardware id
-static const int16_t PROCESSOR_SERIAL = 1001;
+static const uint16_t PROCESSOR_SERIAL = 1001;
 // Task id
-static const int16_t PROCESSOR_TEMPERATURE_KEY = 0;
+static const uint16_t PROCESSOR_TEMPERATURE_KEY = 0;
 // Task id
-static const int16_t PROCESSOR_LOAD_KEY = 1;
+static const uint16_t PROCESSOR_LOAD_KEY = 1;
 
 rcl_ret_t
 my_diagnostic_temperature(diagnostic_value_t * values, uint8_t * number_of_values)

micro_ros_diagnostic_updater/example/example_website_checker.c

@@ -23,7 +23,7 @@
 static int my_diagnostic_status = 0;
 static int my_website_status = 0;
 // Hardware ID
-static const int16_t WEBSITE_SERIAL = 998;
+static const uint16_t WEBSITE_SERIAL = 998;
 // Updater ID
 static const uint16_t WEBSITE_ID = 0;
 // Task ID

micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h

@@ -32,7 +32,7 @@ typedef struct diagnostic_value_t
   bool bool_value;
   int32_t int_value;
   float double_value;
-  int16_t value_id;
+  uint16_t value_id;
 
   int8_t level;
   bool value_has_changed;
@@ -42,16 +42,16 @@ typedef struct diagnostic_task_t
 {
   uint8_t number_of_values;
   diagnostic_value_t values[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK];
-  int16_t hardware_id;
-  int16_t updater_id;
+  uint16_t hardware_id;
+  uint16_t updater_id;
   rcl_ret_t (* function)(
     diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
     uint8_t * number_of_values);
 } diagnostic_task_t;
 
 typedef struct diagnostic_updater_t
 {
-  int16_t id;
+  uint16_t id;
   uint8_t num_tasks;
   diagnostic_task_t * tasks[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER];
   rcl_publisher_t diag_pub;
@@ -75,7 +75,7 @@ void rclc_diagnostic_value_set_bool(
 
 void rclc_diagnostic_value_lookup(
   diagnostic_value_t * kv,
-  int16_t value_id);
+  uint16_t value_id);
 
 void rclc_diagnostic_value_set_level(
   diagnostic_value_t * kv,
@@ -84,8 +84,8 @@ void rclc_diagnostic_value_set_level(
 rcl_ret_t
 rclc_diagnostic_task_init(
   diagnostic_task_t * task,
-  int16_t hardware_id,
-  int16_t updater_id,
+  uint16_t hardware_id,
+  uint16_t updater_id,
   rcl_ret_t (* function)(
     diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
     uint8_t * number_of_values));

micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c

@@ -61,7 +61,7 @@ rclc_diagnostic_value_set_bool(
 void
 rclc_diagnostic_value_lookup(
   diagnostic_value_t * kv,
-  int16_t value_id)
+  uint16_t value_id)
 {
   kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_LOOKUP;
   if (kv->value_id != value_id) {
@@ -84,8 +84,8 @@ rclc_diagnostic_value_set_level(
 rcl_ret_t
 rclc_diagnostic_task_init(
   diagnostic_task_t * task,
-  int16_t hardware_id,
-  int16_t updater_id,
+  uint16_t hardware_id,
+  uint16_t updater_id,
   rcl_ret_t (* function)(
     diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
     uint8_t * number_of_values))