From 67bd39ff2ad58a2b3533440870d8ce68376bad4d Mon Sep 17 00:00:00 2001 From: Daan Wijffels Date: Thu, 5 Dec 2024 14:45:29 +0100 Subject: [PATCH 1/6] Fixed ros2 diagnostics syntax --- depthai_ros_driver/src/dai_nodes/sys_logger.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index cf4dd7b3..00ebc072 100644 --- a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -69,7 +69,21 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& auto logData = loggerQ->get(std::chrono::seconds(5), timeout); if(!timeout) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information"); - stat.add("System Information", sysInfoToString(*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"); } From 5aed7faf179a6843aa0d09b52fc3c1a3b175d568 Mon Sep 17 00:00:00 2001 From: Daan Wijffels Date: Thu, 5 Dec 2024 15:03:18 +0100 Subject: [PATCH 2/6] fixed build error --- depthai_ros_driver/src/dai_nodes/sys_logger.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index 00ebc072..e0ef4b86 100644 --- a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp +++ b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp @@ -69,6 +69,7 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& auto logData = loggerQ->get(std::chrono::seconds(5), timeout); if(!timeout) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information"); + 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)); From fd2c36dd45a1ce97b29dce0d45b25fc028b1c214 Mon Sep 17 00:00:00 2001 From: Jetson Developer Date: Fri, 6 Dec 2024 13:36:21 +0100 Subject: [PATCH 3/6] Get frame_id prefix from paramters when using tf publishing is on --- .../include/depthai_ros_driver/dai_nodes/base_node.hpp | 3 +++ depthai_ros_driver/src/dai_nodes/base_node.cpp | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index d6df3e3d..8be0e2eb 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp @@ -5,6 +5,7 @@ #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/Node.hpp" +#include "depthai_ros_driver/param_handlers/camera_param_handler.hpp" #include "depthai_ros_driver/utils.hpp" #include "rclcpp/logger.hpp" @@ -99,6 +100,8 @@ class BaseNode { std::string baseDAINodeName; bool intraProcessEnabled; rclcpp::Logger logger; + + std::unique_ptr ph; }; } // namespace dai_nodes } // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index b6376bfa..b26a38f1 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -15,6 +15,7 @@ namespace dai_nodes { BaseNode::BaseNode(const std::string& daiNodeName, std::shared_ptr node, std::shared_ptr /*pipeline*/) : baseNode(node), baseDAINodeName(daiNodeName), logger(node->get_logger()) { intraProcessEnabled = node->get_node_options().use_intra_process_comms(); + ph = std::make_unique(baseNode, "camera"); }; BaseNode::~BaseNode() = default; void BaseNode::setNodeName(const std::string& daiNodeName) { @@ -46,6 +47,10 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) { } std::string BaseNode::getTFPrefix(const std::string& frameName) { + if(ph->getParam("i_publish_tf_from_calibration")) { + return ph->getParam("i_tf_base_frame") + "_" + frameName; + } + return std::string(getROSNode()->get_name()) + "_" + frameName; } From ed3a8670005db625e79c0156404fea82c687f6a8 Mon Sep 17 00:00:00 2001 From: Jetson Developer Date: Wed, 11 Dec 2024 10:04:23 +0100 Subject: [PATCH 4/6] Removed CameraParamHandler from BaseNode --- .../include/depthai_ros_driver/dai_nodes/base_node.hpp | 2 -- .../dai_nodes/sensors/sensor_helpers.hpp | 1 + depthai_ros_driver/src/dai_nodes/base_node.cpp | 7 +------ .../src/dai_nodes/sensors/sensor_helpers.cpp | 7 +++++++ 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index 8be0e2eb..cad5ba48 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp @@ -100,8 +100,6 @@ class BaseNode { std::string baseDAINodeName; bool intraProcessEnabled; rclcpp::Logger logger; - - std::unique_ptr ph; }; } // namespace dai_nodes } // namespace depthai_ros_driver 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 index d468e749..230b79d5 100644 --- 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 fSyncModeMap; extern const std::unordered_map cameraImageOrientationMap; bool rsCompabilityMode(std::shared_ptr node); +std::string tfPrefix(std::shared_ptr node); std::string getSocketName(std::shared_ptr node, dai::CameraBoardSocket socket); std::string getNodeName(std::shared_ptr 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 index b26a38f1..8000348f 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -15,7 +15,6 @@ namespace dai_nodes { BaseNode::BaseNode(const std::string& daiNodeName, std::shared_ptr node, std::shared_ptr /*pipeline*/) : baseNode(node), baseDAINodeName(daiNodeName), logger(node->get_logger()) { intraProcessEnabled = node->get_node_options().use_intra_process_comms(); - ph = std::make_unique(baseNode, "camera"); }; BaseNode::~BaseNode() = default; void BaseNode::setNodeName(const std::string& daiNodeName) { @@ -47,11 +46,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) { } std::string BaseNode::getTFPrefix(const std::string& frameName) { - if(ph->getParam("i_publish_tf_from_calibration")) { - return ph->getParam("i_tf_base_frame") + "_" + 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 index c2f51a94..f43ecf33 100644 --- 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 NodeNameMap = { bool rsCompabilityMode(std::shared_ptr node) { return node->get_parameter("camera.i_rs_compat").as_bool(); } + +std::string tfPrefix(std::shared_ptr 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 node, NodeNameEnum name) { if(rsCompabilityMode(node)) { return rsNodeNameMap.at(name); From 1697f077995c6e775394bf58dd7adc2518ea6728 Mon Sep 17 00:00:00 2001 From: Jetson Developer Date: Thu, 12 Dec 2024 16:36:51 +0100 Subject: [PATCH 5/6] hardware_id for diagnostics to fully_qualified_name to allow it to filter on namespace --- depthai_ros_driver/src/dai_nodes/sys_logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_ros_driver/src/dai_nodes/sys_logger.cpp b/depthai_ros_driver/src/dai_nodes/sys_logger.cpp index e0ef4b86..85cb94f6 100644 --- 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 pipeline) { void SysLogger::setupQueues(std::shared_ptr device) { loggerQ = device->getOutputQueue(loggerQName, 8, false); updater = std::make_shared(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)); } From 8fd1faaa75bbf5fac3baa528b2e057820e61e782 Mon Sep 17 00:00:00 2001 From: Jetson Developer Date: Fri, 13 Dec 2024 13:07:52 +0100 Subject: [PATCH 6/6] removed camera_paramter header as it is no longer needed --- .../include/depthai_ros_driver/dai_nodes/base_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index cad5ba48..d6df3e3d 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp @@ -5,7 +5,6 @@ #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/Node.hpp" -#include "depthai_ros_driver/param_handlers/camera_param_handler.hpp" #include "depthai_ros_driver/utils.hpp" #include "rclcpp/logger.hpp"