Fixed diagnostic status key/value parsing

ifm · May 8, 2023 · fbadf57 · fbadf57
1 parent a5711f9
commit fbadf57
Showing 1 changed file with 12 additions and 5 deletions.
diff --git a/src/lib/camera_node.cpp b/src/lib/camera_node.cpp
@@ -1137,12 +1137,19 @@ DiagnosticStatusMsg CameraNode::create_diagnostic_status(const uint8_t level, co
     msg.message = parsed_json["description"];
   }
 
-  for (json::iterator it = parsed_json.begin(); it != parsed_json.end(); it++)
+  for (auto& it : parsed_json.items())
   {
-    diagnostic_msgs::msg::KeyValue obj;
-    obj.key = it.key();
-    obj.value = it.key();
-    msg.values.push_back(obj);
+    try
+    {
+      diagnostic_msgs::msg::KeyValue obj;
+      obj.key = it.key();
+      obj.value = it.value().dump();
+      msg.values.push_back(obj);
+    }
+    catch (const std::exception& e)
+    {
+      RCLCPP_WARN(logger_, "Couldn't parse entry of diagnostics status %s: %s", msg.name, e.what());
+    }
   }
 
   return msg;