Ros2 Diagnostics - Fix for Frame_id naming (#640)

* Fixed ros2 diagnostics syntax * fixed build error * Get frame_id prefix from paramters when using tf publishing is on * Removed CameraParamHandler from BaseNode * hardware_id for diagnostics to fully_qualified_name to allow it to filter on namespace * removed camera_paramter header as it is no longer needed --------- Co-authored-by: Daan Wijffels <[email protected]> Co-authored-by: Jetson Developer <[email protected]>
luxonis · Dec 20, 2024 · fe5285a · fe5285a
1 parent da0463f
commit fe5285a
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 3 deletions.
diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp
@@ -56,6 +56,7 @@ extern const std::unordered_map<std::string, dai::ColorCameraProperties::SensorR
 extern const std::unordered_map<std::string, dai::CameraControl::FrameSyncMode> fSyncModeMap;
 extern const std::unordered_map<std::string, dai::CameraImageOrientation> cameraImageOrientationMap;
 bool rsCompabilityMode(std::shared_ptr<rclcpp::Node> node);
+std::string tfPrefix(std::shared_ptr<rclcpp::Node> node);
 std::string getSocketName(std::shared_ptr<rclcpp::Node> node, dai::CameraBoardSocket socket);
 std::string getNodeName(std::shared_ptr<rclcpp::Node> node, NodeNameEnum name);
 void basicCameraPub(const std::string& /*name*/,

diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp
@@ -46,7 +46,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) {
 }
 
 std::string BaseNode::getTFPrefix(const std::string& frameName) {
-    return std::string(getROSNode()->get_name()) + "_" + frameName;
+    return sensor_helpers::tfPrefix(getROSNode()) + "_" + frameName;
 }
 
 std::string BaseNode::getOpticalTFPrefix(const std::string& frameName) {

diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp
@@ -63,6 +63,13 @@ const std::unordered_map<NodeNameEnum, std::string> NodeNameMap = {
 bool rsCompabilityMode(std::shared_ptr<rclcpp::Node> node) {
     return node->get_parameter("camera.i_rs_compat").as_bool();
 }
+
+std::string tfPrefix(std::shared_ptr<rclcpp::Node> node) {
+    if(node->get_parameter("camera.i_publish_tf_from_calibration").as_bool()) {
+        return node->get_parameter("camera.i_tf_base_frame").as_string();
+    }
+    return node->get_name();
+}
 std::string getNodeName(std::shared_ptr<rclcpp::Node> node, NodeNameEnum name) {
     if(rsCompabilityMode(node)) {
         return rsNodeNameMap.at(name);

diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp
@@ -33,7 +33,7 @@ void SysLogger::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
 void SysLogger::setupQueues(std::shared_ptr<dai::Device> device) {
     loggerQ = device->getOutputQueue(loggerQName, 8, false);
     updater = std::make_shared<diagnostic_updater::Updater>(getROSNode());
-    updater->setHardwareID(getROSNode()->get_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName());
+    updater->setHardwareID(getROSNode()->get_fully_qualified_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName());
     updater->add("sys_logger", std::bind(&SysLogger::produceDiagnostics, this, std::placeholders::_1));
 }
 
@@ -69,7 +69,22 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper&
         auto logData = loggerQ->get<dai::SystemInformation>(std::chrono::seconds(5), timeout);
         if(!timeout) {
             stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information");
-            stat.add("System Information", sysInfoToString(*logData));
+            const dai::SystemInformation& sysInfo = *logData;
+            stat.add("Leon CSS CPU Usage", sysInfo.leonCssCpuUsage.average * 100);
+            stat.add("Leon MSS CPU Usage", sysInfo.leonMssCpuUsage.average * 100);
+            stat.add("Ddr Memory Usage", sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Ddr Memory Total", sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Cmx Memory Usage", sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Cmx Memory Total", sysInfo.cmxMemoryUsage.total);
+            stat.add("Leon CSS Memory Usage", sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Leon CSS Memory Total", sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Leon MSS Memory Usage", sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f));
+            stat.add("Leon MSS Memory Total", sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f));
+            stat.add("Average Chip Temperature", sysInfo.chipTemperature.average);
+            stat.add("Leon CSS Chip Temperature", sysInfo.chipTemperature.css);
+            stat.add("Leon MSS Chip Temperature", sysInfo.chipTemperature.mss);
+            stat.add("UPA Chip Temperature", sysInfo.chipTemperature.upa);
+            stat.add("DSS Chip Temperature", sysInfo.chipTemperature.dss);
         } else {
             stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No Data");
         }