diff --git a/NOTICE b/NOTICE index 0ab1cd3..42aa8e8 100644 --- a/NOTICE +++ b/NOTICE @@ -24,5 +24,7 @@ Capra Robotics ApS Bartolome Jimenez Vera +Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com> + Robert Bosch GmbH Arne Nordmann diff --git a/micro_ros_diagnostic_bridge/include/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp b/micro_ros_diagnostic_bridge/include/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp index 0155c2e..6e837be 100644 --- a/micro_ros_diagnostic_bridge/include/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp +++ b/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 +#include #include #include #include @@ -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 HardwareMap; -typedef std::map UpdaterMap; +typedef std::map HardwareMap; +typedef std::map UpdaterMap; typedef std::map KeyMap; typedef std::map 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); diff --git a/micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp b/micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp index 0311443..bf47459 100644 --- a/micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp +++ b/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 #include #include #include @@ -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::max()) + { + throw std::out_of_range("index is outside the uint16 range"); + } + return static_cast(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()); } } diff --git a/micro_ros_diagnostic_bridge/test/test_diagnostic_bridge.cpp b/micro_ros_diagnostic_bridge/test/test_diagnostic_bridge.cpp index f3ba8a7..2a9ec90 100644 --- a/micro_ros_diagnostic_bridge/test/test_diagnostic_bridge.cpp +++ b/micro_ros_diagnostic_bridge/test/test_diagnostic_bridge.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -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::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,6 +83,7 @@ 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)); @@ -86,6 +91,7 @@ TEST_F(TestDiagnosticBridge, translating) { // 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)); diff --git a/micro_ros_diagnostic_bridge/test/test_lookup_table.yaml b/micro_ros_diagnostic_bridge/test/test_lookup_table.yaml index 30a316f..8cf54cc 100644 --- a/micro_ros_diagnostic_bridge/test/test_lookup_table.yaml +++ b/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" diff --git a/micro_ros_diagnostic_updater/example/example_processor_updater.c b/micro_ros_diagnostic_updater/example/example_processor_updater.c index 0d9c52f..832cb56 100644 --- a/micro_ros_diagnostic_updater/example/example_processor_updater.c +++ b/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) diff --git a/micro_ros_diagnostic_updater/example/example_website_checker.c b/micro_ros_diagnostic_updater/example/example_website_checker.c index b56e299..29af073 100644 --- a/micro_ros_diagnostic_updater/example/example_website_checker.c +++ b/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 diff --git a/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h b/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h index ad73a12..e75da63 100644 --- a/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h +++ b/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,8 +42,8 @@ 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); @@ -51,7 +51,7 @@ typedef struct 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)); diff --git a/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c b/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c index 4a6c9eb..4e73d81 100644 --- a/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c +++ b/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)) diff --git a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp b/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp index fdfde4e..29885b5 100644 --- a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp +++ b/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp @@ -15,6 +15,7 @@ // limitations under the License. #include #include +#include #include @@ -93,6 +94,21 @@ TEST(TestDiagnosticUpdater, create_diagnostic_values) { EXPECT_EQ( value.level, micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__WARN); + + rclc_diagnostic_value_lookup(&value, std::numeric_limits::max()); + EXPECT_EQ(value.value_id, std::numeric_limits::max()); +} + +TEST(TestDiagnosticUpdater, task_ids_use_full_uint16_range) { + diagnostic_task_t task; + const uint16_t max_id = std::numeric_limits::max(); + + rcl_ret_t rc = rclc_diagnostic_task_init( + &task, max_id, max_id, &update_function_mockup_0); + + EXPECT_EQ(RCL_RET_OK, rc); + EXPECT_EQ(max_id, task.hardware_id); + EXPECT_EQ(max_id, task.updater_id); } TEST(TestDiagnosticUpdater, create_updater) {