Watchdog humble

depthai_ros_driver/CMakeLists.txt

@@ -28,6 +28,8 @@ depthai_bridge
 image_transport
 rclcpp
 sensor_msgs
+diagnostic_updater
+diagnostic_msgs
 )
 
 set(SENSOR_DEPS
@@ -63,6 +65,7 @@ add_library(
   ${COMMON_LIB_NAME} SHARED
   src/utils.cpp
   src/dai_nodes/base_node.cpp
+  src/dai_nodes/sys_logger.cpp
   src/dai_nodes/sensors/sensor_helpers.cpp # TODO: Figure out different place for this 
   src/param_handlers/camera_param_handler.cpp
   src/param_handlers/imu_param_handler.cpp

depthai_ros_driver/include/depthai_ros_driver/camera.hpp

@@ -8,6 +8,7 @@
 #include "depthai_ros_driver/param_handlers/camera_param_handler.hpp"
 #include "rclcpp/node.hpp"
 #include "std_srvs/srv/trigger.hpp"
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
 
 namespace dai {
 class Pipeline;
@@ -36,15 +37,27 @@ class Camera : public rclcpp::Node {
     OnSetParametersCallbackHandle::SharedPtr paramCBHandle;
     std::unique_ptr<param_handlers::CameraParamHandler> ph;
     rclcpp::Service<Trigger>::SharedPtr startSrv, stopSrv, savePipelineSrv, saveCalibSrv;
+    rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagSub;
+    /*
+    * Closes all the queues, clears the configured BaseNodes, stops the pipeline and resets the device.
+    */
+    void stop();
+    /*
+    * Runs onConfigure();
+    */
+    void start();
+    void restart();
+    void diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg);
+
     void startCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res);
     void stopCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res);
     void saveCalibCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res);
     void savePipelineCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res);
-
     std::vector<std::string> usbStrings = {"UNKNOWN", "LOW", "FULL", "HIGH", "SUPER", "SUPER_PLUS"};
     std::shared_ptr<dai::Pipeline> pipeline;
     std::shared_ptr<dai::Device> device;
     std::vector<std::unique_ptr<dai_nodes::BaseNode>> daiNodes;
     bool camRunning = false;
+
 };
 }  // namespace depthai_ros_driver

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp

@@ -21,8 +21,8 @@ class BaseNode {
    public:
     BaseNode(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr<dai::Pipeline> pipeline);
     virtual ~BaseNode();
-    virtual void updateParams(const std::vector<rclcpp::Parameter>& params) = 0;
-    virtual void link(dai::Node::Input in, int linkType = 0) = 0;
+    virtual void updateParams(const std::vector<rclcpp::Parameter>& params);
+    virtual void link(dai::Node::Input in, int linkType = 0);
     virtual dai::Node::Input getInput(int linkType = 0);
     virtual void setupQueues(std::shared_ptr<dai::Device> device) = 0;
     virtual void setNames() = 0;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sys_logger.hpp

@@ -0,0 +1,43 @@
+#include "depthai_ros_driver/dai_nodes/base_node.hpp"
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "depthai/pipeline/datatype/SystemInformation.hpp"
+namespace dai {
+class Pipeline;
+class Device;
+class DataOutputQueue;
+class ADatatype;
+namespace node {
+class SystemLogger;
+class XLinkOut;
+class VideoEncoder;
+}  // namespace node
+}  // namespace dai
+namespace rclcpp {
+class Node;
+class TimerBase;
+}  // namespace rclcpp
+
+namespace depthai_ros_driver {
+
+
+namespace dai_nodes {
+    class SysLogger : public BaseNode {
+       public:
+        SysLogger(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr<dai::Pipeline> pipeline);
+        ~SysLogger();
+        void setupQueues(std::shared_ptr<dai::Device> device) override;
+        void setNames() override;
+        void setXinXout(std::shared_ptr<dai::Pipeline> pipeline) override;
+        void closeQueues() override;
+
+       private:
+        std::string sysInfoToString(const dai::SystemInformation& sysInfo);
+        void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
+        std::shared_ptr<diagnostic_updater::Updater> updater;
+        std::shared_ptr<dai::node::XLinkOut> xoutLogger;
+        std::shared_ptr<dai::node::SystemLogger> sysNode;
+        std::shared_ptr<dai::DataOutputQueue> loggerQ;
+        std::string loggerQName;
+    };
+}
+}

depthai_ros_driver/package.xml

@@ -16,7 +16,6 @@
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>vision_msgs</depend>
-  <depend>diagnostic_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>cv_bridge</depend>
@@ -29,7 +28,8 @@
   <depend>depthai_ros_msgs</depend>
   <depend>depthai_examples</depend>
   <depend>pluginlib</depend>
-
+  <depend>diagnostic_updater</depend>
+  <depend>diagnostic_msgs</depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

depthai_ros_driver/src/camera.cpp

@@ -6,6 +6,7 @@
 #include "depthai/device/Device.hpp"
 #include "depthai/pipeline/Pipeline.hpp"
 #include "depthai_ros_driver/pipeline/pipeline_generator.hpp"
+#include "diagnostic_msgs/msg/diagnostic_status.hpp"
 
 namespace depthai_ros_driver {
 
@@ -25,15 +26,63 @@ void Camera::onConfigure() {
     stopSrv = this->create_service<Trigger>("~/stop_camera", std::bind(&Camera::stopCB, this, std::placeholders::_1, std::placeholders::_2));
     savePipelineSrv = this->create_service<Trigger>("~/save_pipeline", std::bind(&Camera::savePipelineCB, this, std::placeholders::_1, std::placeholders::_2));
     saveCalibSrv = this->create_service<Trigger>("~/save_calibration", std::bind(&Camera::saveCalibCB, this, std::placeholders::_1, std::placeholders::_2));
+    diagSub = this->create_subscription<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10, std::bind(&Camera::diagCB, this, std::placeholders::_1));
     RCLCPP_INFO(this->get_logger(), "Camera ready!");
 }
 
+void Camera::diagCB(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) {
+    for(const auto& status : msg->status) {
+        if(status.name == get_name() + std::string(": sys_logger")) {
+            if(status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) {
+                RCLCPP_ERROR(this->get_logger(), "Camera diagnostics error: %s", status.message.c_str());
+                restart();
+            }
+        }
+    }
+}
+
+void Camera::start() {
+    RCLCPP_INFO(this->get_logger(), "Starting camera.");
+    if(!camRunning) {
+        onConfigure();
+    } else {
+        RCLCPP_INFO(this->get_logger(), "Camera already running!.");
+    }
+}
+
+void Camera::stop() {
+    RCLCPP_INFO(this->get_logger(), "Stopping camera.");
+    if(camRunning) {
+        for(const auto& node : daiNodes) {
+            node->closeQueues();
+        }
+        daiNodes.clear();
+        device.reset();
+        pipeline.reset();
+        camRunning = false;
+    } else {
+        RCLCPP_INFO(this->get_logger(), "Camera already stopped!");
+    }
+}
+
+void Camera::restart() {
+    RCLCPP_ERROR(this->get_logger(), "Restarting camera");
+    stop();
+    start();
+    if(camRunning) {
+        return;
+    } else {
+        RCLCPP_ERROR(this->get_logger(), "Restarting camera failed.");
+    }
+}
+
 void Camera::saveCalib() {
     auto calibHandler = device->readCalibration();
     std::stringstream savePath;
     savePath << "/tmp/" << device->getMxId().c_str() << "_calibration.json";
     RCLCPP_INFO(this->get_logger(), "Saving calibration to: %s", savePath.str().c_str());
     calibHandler.eepromToJsonFile(savePath.str());
+    auto json = calibHandler.eepromToJson();
 }
 
 void Camera::loadCalib(const std::string& path) {
@@ -62,27 +111,11 @@ void Camera::savePipelineCB(const Trigger::Request::SharedPtr /*req*/, Trigger::
 }
 
 void Camera::startCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res) {
-    RCLCPP_INFO(this->get_logger(), "Starting camera.");
-    if(!camRunning) {
-        onConfigure();
-    } else {
-        RCLCPP_INFO(this->get_logger(), "Camera already running!.");
-    }
+    start();
     res->success = true;
 }
 void Camera::stopCB(const Trigger::Request::SharedPtr /*req*/, Trigger::Response::SharedPtr res) {
-    RCLCPP_INFO(this->get_logger(), "Stopping camera.");
-    if(camRunning) {
-        for(const auto& node : daiNodes) {
-            node->closeQueues();
-        }
-        daiNodes.clear();
-        device.reset();
-        pipeline.reset();
-        camRunning = false;
-    } else {
-        RCLCPP_INFO(this->get_logger(), "Camera already stopped!");
-    }
+    stop();
     res->success = true;
 }
 void Camera::getDeviceType() {

depthai_ros_driver/src/dai_nodes/base_node.cpp

@@ -29,5 +29,29 @@ std::string BaseNode::getTFPrefix(const std::string& frameName) {
 dai::Node::Input BaseNode::getInput(int /*linkType = 0*/) {
     throw(std::runtime_error("getInput() not implemented"));
 };
+
+void BaseNode::closeQueues() {
+    throw(std::runtime_error("closeQueues() not implemented"));
+};
+
+void BaseNode::setNames() {
+    throw(std::runtime_error("setNames() not implemented"));
+};
+
+void BaseNode::setXinXout(std::shared_ptr<dai::Pipeline> /*pipeline*/) {
+    throw(std::runtime_error("setXinXout() not implemented"));
+};
+
+void BaseNode::setupQueues(std::shared_ptr<dai::Device> /*device*/) {
+    throw(std::runtime_error("setupQueues() not implemented"));
+};
+
+void BaseNode::link(dai::Node::Input /*in*/, int /*linkType = 0*/) {
+    throw(std::runtime_error("link() not implemented"));
+};
+
+void BaseNode::updateParams(const std::vector<rclcpp::Parameter>& /*params*/) {
+    return;
+};
 }  // namespace dai_nodes
 }  // namespace depthai_ros_driver

depthai_ros_driver/src/dai_nodes/sys_logger.cpp

@@ -0,0 +1,81 @@
+#include "depthai_ros_driver/dai_nodes/sys_logger.hpp"
+
+#include "depthai/device/DataQueue.hpp"
+#include "depthai/device/Device.hpp"
+#include "depthai/pipeline/Pipeline.hpp"
+#include "depthai/pipeline/datatype/SystemInformation.hpp"
+#include "depthai/pipeline/node/SystemLogger.hpp"
+#include "depthai/pipeline/node/XLinkOut.hpp"
+#include "rclcpp/node.hpp"
+
+namespace depthai_ros_driver {
+namespace dai_nodes {
+SysLogger::SysLogger(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr<dai::Pipeline> pipeline) : BaseNode(daiNodeName, node, pipeline) {
+    RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str());
+    setNames();
+    sysNode = pipeline->create<dai::node::SystemLogger>();
+    setXinXout(pipeline);
+    RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str());
+}
+SysLogger::~SysLogger() = default;
+
+void SysLogger::setNames() {
+    loggerQName = getName() + "_queue";
+}
+
+void SysLogger::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
+    xoutLogger = pipeline->create<dai::node::XLinkOut>();
+    xoutLogger->setStreamName(loggerQName);
+    sysNode->out.link(xoutLogger->input);
+}
+
+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->add("sys_logger", std::bind(&SysLogger::produceDiagnostics, this, std::placeholders::_1));
+}
+
+void SysLogger::closeQueues() {
+    loggerQ->close();
+}
+
+std::string SysLogger::sysInfoToString(const dai::SystemInformation& sysInfo) {
+    std::stringstream ss;
+    ss << "System Information: " << std::endl;
+    ss << "  Leon CSS CPU Usage: " << sysInfo.leonCssCpuUsage.average * 100 << std::endl;
+    ss << "  Leon MSS CPU Usage: " << sysInfo.leonMssCpuUsage.average * 100 << std::endl;
+    ss << " Ddr Memory Usage: " << sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f) << std::endl;
+    ss << " Ddr Memory Total: " << sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f) << std::endl;
+    ss << " Cmx Memory Usage: " << sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f) << std::endl;
+    ss << " Cmx Memory Total: " << sysInfo.cmxMemoryUsage.total << std::endl;
+    ss << " Leon CSS Memory Usage: " << sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f) << std::endl;
+    ss << " Leon CSS Memory Total: " << sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f) << std::endl;
+    ss << " Leon MSS Memory Usage: " << sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f) << std::endl;
+    ss << " Leon MSS Memory Total: " << sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f) << std::endl;
+    ss << " Average Chip Temperature: " << sysInfo.chipTemperature.average << std::endl;
+    ss << " Leon CSS Chip Temperature: " << sysInfo.chipTemperature.css << std::endl;
+    ss << " Leon MSS Chip Temperature: " << sysInfo.chipTemperature.mss << std::endl;
+    ss << " UPA Chip Temperature: " << sysInfo.chipTemperature.upa << std::endl;
+    ss << " DSS Chip Temperature: " << sysInfo.chipTemperature.dss << std::endl;
+
+    return ss.str();
+}
+
+void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) {
+    try {
+        auto logData = loggerQ->tryGet<dai::SystemInformation>();
+        if(logData) {
+            stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information");
+            stat.add("System Information", sysInfoToString(*logData));
+        } else {
+            stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No Data");
+        }
+    } catch(const std::exception& e) {
+        RCLCPP_ERROR(getROSNode()->get_logger(), "No data on logger queue!");
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, e.what());
+    }
+}
+
+}  // namespace dai_nodes
+}  // namespace depthai_ros_driver

depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp

@@ -99,6 +99,9 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
         stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam<int>("i_subpixel_fractional_bits", 3));
     }
     stereo->setRectifyEdgeFillColor(declareAndLogParam<int>("i_rectify_edge_fill_color", 0));
+    if(declareAndLogParam<bool>("i_enable_alpha_scaling", false)){
+        stereo->setAlphaScaling(declareAndLogParam<float>("i_alpha_scaling", 0.0));
+    }
     auto config = stereo->initialConfig.get();
     config.costMatching.disparityWidth = utils::getValFromMap(declareAndLogParam<std::string>("i_disparity_width", "DISPARITY_96"), disparityWidthMap);
     stereo->setExtendedDisparity(declareAndLogParam<bool>("i_extended_disp", false));
@@ -114,6 +117,9 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
     if(config.postProcessing.speckleFilter.enable) {
         config.postProcessing.speckleFilter.speckleRange = declareAndLogParam<int>("i_speckle_filter_speckle_range", 50);
     }
+    if(declareAndLogParam<bool>("i_enable_disparity_shift", false)){
+        config.algorithmControl.disparityShift = declareAndLogParam<int>("i_disparity_shift", 0);
+    }
     config.postProcessing.spatialFilter.enable = declareAndLogParam<bool>("i_enable_spatial_filter", false);
     if(config.postProcessing.spatialFilter.enable) {
         config.postProcessing.spatialFilter.holeFillingRadius = declareAndLogParam<int>("i_spatial_filter_hole_filling_radius", 2);

depthai_ros_driver/src/pipeline/pipeline_generator.cpp

@@ -3,6 +3,7 @@
 #include "depthai/device/Device.hpp"
 #include "depthai/pipeline/Pipeline.hpp"
 #include "depthai_ros_driver/dai_nodes/sensors/imu.hpp"
+#include "depthai_ros_driver/dai_nodes/sys_logger.hpp"
 #include "depthai_ros_driver/pipeline/base_pipeline.hpp"
 #include "depthai_ros_driver/utils.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -45,7 +46,8 @@ std::vector<std::unique_ptr<dai_nodes::BaseNode>> PipelineGenerator::createPipel
             daiNodes.push_back(std::move(imu));
         }
     }
-
+    auto sysLogger = std::make_unique<dai_nodes::SysLogger>("sys_logger", node, pipeline);
+    daiNodes.push_back(std::move(sysLogger));
     RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline.");
     return daiNodes;
 }