format, ir update

depthai_bridge/include/depthai_bridge/ImageConverter.hpp

@@ -9,14 +9,14 @@
 #include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai-shared/common/Point2f.hpp"
 #include "depthai/device/CalibrationHandler.hpp"
-#include "depthai/pipeline/datatype/ImgFrame.hpp"
 #include "depthai/pipeline/datatype/EncodedFrame.hpp"
+#include "depthai/pipeline/datatype/ImgFrame.hpp"
+#include "depthai_ros_msgs/FFMPEGPacket.h"
 #include "ros/time.h"
 #include "sensor_msgs/CameraInfo.h"
-#include "sensor_msgs/Image.h"
 #include "sensor_msgs/CompressedImage.h"
+#include "sensor_msgs/Image.h"
 #include "std_msgs/Header.h"
-#include "depthai_ros_msgs/FFMPEGPacket.h"
 
 namespace dai {
 
@@ -30,7 +30,6 @@ using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono
 using FFMPegImagePtr = DepthAiRosMsgs::FFMPEGPacketPtr;
 using CompImagePtr = ImageMsgs::CompressedImagePtr;
 
-
 class ImageConverter {
    public:
     // ImageConverter() = default;
@@ -84,7 +83,7 @@ class ImageConverter {
      */
     void setAlphaScaling(double alphaScalingFactor = 0.0);
 
-	/**
+    /**
      * @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264.
      * @param encoding: The encoding to be used.
      */
@@ -94,11 +93,10 @@ class ImageConverter {
     void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
     ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);
 
-	DepthAiRosMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);
+    DepthAiRosMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);
 
     ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData);
 
-
     void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);
 
     /** TODO(sachin): Add support for ros msg to cv mat since we have some
@@ -139,7 +137,7 @@ class ImageConverter {
     double baseline;
     bool alphaScalingEnabled = false;
     double alphaScalingFactor = 0.0;
-	int camHeight = -1;
+    int camHeight = -1;
     int camWidth = -1;
     std::string ffmpegEncoding = "libx264";
 };

depthai_filters/include/depthai_filters/wls_filter.hpp

@@ -31,6 +31,6 @@ class WLSFilter : public nodelet::Nodelet {
     cv::Ptr<cv::ximgproc::DisparityWLSFilter> filter;
     image_transport::CameraPublisher depthPub;
     std::shared_ptr<image_transport::ImageTransport> it;
-	double maxDisparity;
+    double maxDisparity;
 };
 }  // namespace depthai_filters

depthai_filters/src/wls_filter.cpp

@@ -28,7 +28,7 @@ void WLSFilter::onInit() {
 void WLSFilter::parameterCB(wlsConfig& config, uint32_t level) {
     filter->setLambda(config.lambda);
     filter->setSigmaColor(config.sigma);
-	maxDisparity = config.max_disparity;
+    maxDisparity = config.max_disparity;
 }
 
 void WLSFilter::wlsCB(const sensor_msgs::ImageConstPtr& disp, const sensor_msgs::CameraInfoConstPtr& disp_info, const sensor_msgs::ImageConstPtr& leftImg) {

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp

@@ -70,7 +70,7 @@ class SpatialDetection : public BaseNode {
         detPub = getROSNode().template advertise<vision_msgs::Detection3DArray>(getName() + "/spatial_detections", 10);
 
         if(ph->getParam<bool>("i_enable_passthrough")) {
-			            utils::ImgConverterConfig convConf;
+            utils::ImgConverterConfig convConf;
             convConf.tfPrefix = tfPrefix;
             convConf.getBaseDeviceTimestamp = ph->getParam<bool>("i_get_base_device_timestamp");
             convConf.updateROSBaseTimeOnRosMsg = ph->getParam<bool>("i_update_ros_base_time_on_ros_msg");
@@ -87,7 +87,7 @@ class SpatialDetection : public BaseNode {
         }
 
         if(ph->getParam<bool>("i_enable_passthrough_depth")) {
-			            dai::CameraBoardSocket socket = static_cast<dai::CameraBoardSocket>(ph->getOtherNodeParam<int>("stereo", "i_board_socket_id"));
+            dai::CameraBoardSocket socket = static_cast<dai::CameraBoardSocket>(ph->getOtherNodeParam<int>("stereo", "i_board_socket_id"));
             if(!ph->getOtherNodeParam<bool>("stereo", "i_align_depth")) {
                 tfPrefix = getTFPrefix("right");
             };
@@ -129,7 +129,7 @@ class SpatialDetection : public BaseNode {
         xoutNN = pipeline->create<dai::node::XLinkOut>();
         xoutNN->setStreamName(nnQName);
         spatialNode->out.link(xoutNN->input);
-		        if(ph->getParam<bool>("i_enable_passthrough")) {
+        if(ph->getParam<bool>("i_enable_passthrough")) {
             ptPub = setupOutput(pipeline, ptQName, [&](dai::Node::Input input) { spatialNode->passthrough.link(input); });
         }
         if(ph->getParam<bool>("i_enable_passthrough_depth")) {
@@ -160,7 +160,7 @@ class SpatialDetection : public BaseNode {
     std::unique_ptr<dai::ros::SpatialDetectionConverter> detConverter;
     std::vector<std::string> labelNames;
     ros::Publisher detPub;
-	std::shared_ptr<sensor_helpers::ImagePublisher> ptPub, ptDepthPub;
+    std::shared_ptr<sensor_helpers::ImagePublisher> ptPub, ptDepthPub;
     std::shared_ptr<T> spatialNode;
     std::shared_ptr<dai::node::ImageManip> imageManip;
     std::unique_ptr<param_handlers::NNParamHandler> ph;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/img_pub.hpp

@@ -7,10 +7,10 @@
 #include "depthai_ros_driver/utils.hpp"
 #include "depthai_ros_msgs/FFMPEGPacket.h"
 #include "image_transport/camera_publisher.h"
+#include "image_transport/image_transport.h"
 #include "sensor_msgs/CameraInfo.h"
 #include "sensor_msgs/CompressedImage.h"
 #include "sensor_msgs/Image.h"
-#include "image_transport/image_transport.h"
 
 namespace dai {
 class Device;
@@ -25,7 +25,6 @@ class ImageConverter;
 }
 }  // namespace dai
 
-
 namespace camera_info_manager {
 class CameraInfoManager;
 }
@@ -85,9 +84,8 @@ class ImagePublisher {
     std::shared_ptr<dai::node::VideoEncoder> createEncoder(std::shared_ptr<dai::Pipeline> pipeline, const utils::VideoEncoderConfig& encoderConfig);
 
    private:
-    bool detectSubscription(const ros::Publisher pub,
-                            const ros::Publisher infoPub);
-	ros::NodeHandle node;
+    bool detectSubscription(const ros::Publisher pub, const ros::Publisher infoPub);
+    ros::NodeHandle node;
     image_transport::ImageTransport it;
     utils::VideoEncoderConfig encConfig;
     utils::ImgPublisherConfig pubConfig;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp

@@ -16,7 +16,6 @@ namespace ros {
 class NodeHandle;
 }  // namespace ros
 
-
 namespace depthai_ros_driver {
 namespace param_handlers {
 class SensorParamHandler;
@@ -25,7 +24,7 @@ namespace dai_nodes {
 namespace sensor_helpers {
 struct ImageSensor;
 class ImagePublisher;
-} // namespace sensor_helpers
+}  // namespace sensor_helpers
 
 class Mono : public BaseNode {
    public:
@@ -42,10 +41,10 @@ class Mono : public BaseNode {
     void setNames() override;
     void setXinXout(std::shared_ptr<dai::Pipeline> pipeline) override;
     void closeQueues() override;
-	std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
+    std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
 
    private:
-	std::shared_ptr<sensor_helpers::ImagePublisher> imagePublisher;
+    std::shared_ptr<sensor_helpers::ImagePublisher> imagePublisher;
     std::shared_ptr<dai::node::MonoCamera> monoCamNode;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
     std::shared_ptr<dai::DataInputQueue> controlQ;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp

@@ -19,7 +19,6 @@ namespace ros {
 class NodeHandle;
 }  // namespace ros
 
-
 namespace depthai_ros_driver {
 namespace param_handlers {
 class SensorParamHandler;
@@ -46,10 +45,10 @@ class RGB : public BaseNode {
     void setNames() override;
     void setXinXout(std::shared_ptr<dai::Pipeline> pipeline) override;
     void closeQueues() override;
-	std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
+    std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
 
    private:
-	std::shared_ptr<sensor_helpers::ImagePublisher> rgbPub, previewPub;
+    std::shared_ptr<sensor_helpers::ImagePublisher> rgbPub, previewPub;
     std::shared_ptr<dai::node::ColorCamera> colorCamNode;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
     std::shared_ptr<dai::DataInputQueue> controlQ;

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sync.hpp

@@ -22,7 +22,7 @@ class XLinkOut;
 namespace ros {
 class NodeHandle;
 class Parameter;
-}  // namespace rclcpp
+}  // namespace ros
 
 namespace depthai_ros_driver {
 namespace param_handlers {

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp

@@ -19,7 +19,7 @@ class XLinkIn;
 namespace ros {
 class NodeHandle;
 class Parameter;
-}  // namespace rclcpp
+}  // namespace ros
 
 namespace depthai_ros_driver {
 namespace param_handlers {

depthai_ros_driver/include/depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp

@@ -13,7 +13,7 @@ enum class UsbSpeed;
 namespace ros {
 class NodeHandle;
 class Parameter;
-}  // namespace rclcpp
+}  // namespace ros
 
 namespace depthai_ros_driver {
 namespace param_handlers {

depthai_ros_driver/include/depthai_ros_driver/param_handlers/sync_param_handler.hpp

@@ -17,7 +17,7 @@ class Sync;
 namespace ros {
 class NodeHandle;
 class Parameter;
-}  // namespace rclcpp
+}  // namespace ros
 
 namespace depthai_ros_driver {
 namespace param_handlers {

depthai_ros_driver/src/camera.cpp

@@ -19,7 +19,7 @@ void Camera::onInit() {
     ph->declareParams();
     paramServer = std::make_shared<dynamic_reconfigure::Server<parametersConfig>>(pNH);
     paramServer->setCallback(std::bind(&Camera::parameterCB, this, std::placeholders::_1, std::placeholders::_2));
-    onConfigure();
+    start();
     startSrv = pNH.advertiseService("start_camera", &Camera::startCB, this);
     stopSrv = pNH.advertiseService("stop_camera", &Camera::stopCB, this);
     savePipelineSrv = pNH.advertiseService("save_pipeline", &Camera::startCB, this);
@@ -68,14 +68,22 @@ void Camera::diagCB(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) {
 }
 
 void Camera::start() {
-    ROS_INFO("Starting camera.");
-    if(!camRunning) {
-        onConfigure();
-    } else {
-        ROS_INFO("Camera already running!.");
+    bool success = false;
+    while(!success) {
+        try {
+            ROS_INFO("Starting camera.");
+            if(!camRunning) {
+                onConfigure();
+            } else {
+                ROS_INFO("Camera already running!.");
+            }
+            success = true;
+        } catch(const std::exception& e) {
+            ROS_ERROR("Exception occurred: %s. Retry", e.what());
+            camRunning = false;
+        }
     }
 }
-
 void Camera::stop() {
     ROS_INFO("Stopping camera.");
     if(camRunning) {
@@ -152,10 +160,7 @@ void Camera::parameterCB(parametersConfig& config, uint32_t /*level*/) {
     enableIR = config.camera_i_enable_ir;
     floodlightBrighness = config.camera_i_floodlight_brightness;
     laserDotBrightness = config.camera_i_laser_dot_brightness;
-    if(camRunning && enableIR && !device->getIrDrivers().empty()) {
-        device->setIrFloodLightBrightness(floodlightBrighness);
-        device->setIrLaserDotProjectorBrightness(laserDotBrightness);
-    }
+    setIR();
     if(!daiNodes.empty()) {
         for(const auto& node : daiNodes) {
             try {
@@ -175,7 +180,7 @@ void Camera::onConfigure() {
         setupQueues();
         setIR();
     } catch(const std::runtime_error& e) {
-        ROS_ERROR(e.what());
+        ROS_ERROR("%s", e.what());
         throw std::runtime_error("Failed to start up the camera.");
     }
     ROS_INFO("Camera ready!");
@@ -199,8 +204,7 @@ void Camera::getDeviceType() {
 
 void Camera::createPipeline() {
     auto generator = std::make_unique<pipeline_gen::PipelineGenerator>();
-    daiNodes = generator->createPipeline(
-        pNH, device, pipeline, ph->getParam<std::string>("i_pipeline_type"), ph->getParam<std::string>("i_nn_type"));
+    daiNodes = generator->createPipeline(pNH, device, pipeline, ph->getParam<std::string>("i_pipeline_type"), ph->getParam<std::string>("i_nn_type"));
     if(ph->getParam<bool>("i_pipeline_dump")) {
         savePipeline();
     }
@@ -282,8 +286,18 @@ void Camera::startDevice() {
 
 void Camera::setIR() {
     if(camRunning && enableIR && !device->getIrDrivers().empty()) {
-        device->setIrFloodLightBrightness(floodlightBrighness);
-        device->setIrLaserDotProjectorBrightness(laserDotBrightness);
+        float laserdotIntensity = float(laserDotBrightness);
+        if(laserdotIntensity > 1.0) {
+            laserdotIntensity =  laserdotIntensity / 1200.0;
+        }
+		ROS_INFO("Setting IR laser dot intensity to: %f", laserdotIntensity);
+        device->setIrLaserDotProjectorIntensity(laserdotIntensity);
+        float floodlightIntensity = float(floodlightBrighness);
+        if(floodlightIntensity > 1.0) {
+            floodlightIntensity =  floodlightIntensity / 1500.0;
+        }
+		ROS_INFO("Setting IR floodlight intensity to: %f", floodlightIntensity);
+        device->setIrFloodLightIntensity(floodlightIntensity);
     }
 }
 

depthai_ros_driver/src/dai_nodes/base_node.cpp

@@ -2,8 +2,8 @@
 
 #include "depthai/device/Device.hpp"
 #include "depthai/pipeline/Pipeline.hpp"
-#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
 #include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp"
+#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
 #include "ros/node_handle.h"
 
 namespace depthai_ros_driver {

depthai_ros_driver/src/dai_nodes/sensors/mono.cpp

@@ -66,7 +66,7 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
 
         utils::ImgPublisherConfig pubConf;
         pubConf.daiNodeName = getName();
-        pubConf.topicName =  getName();
+        pubConf.topicName = getName();
         pubConf.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
         pubConf.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
         pubConf.calibrationFile = ph->getParam<std::string>("i_calibration_file");

depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp

@@ -78,7 +78,7 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {
 
         utils::ImgPublisherConfig pubConfig;
         pubConfig.daiNodeName = getName();
-        pubConfig.topicName =  getName();
+        pubConfig.topicName = getName();
         pubConfig.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
         pubConfig.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
         pubConfig.calibrationFile = ph->getParam<std::string>("i_calibration_file");

depthai_ros_driver/src/dai_nodes/sensors/sync.cpp

@@ -13,8 +13,7 @@
 namespace depthai_ros_driver {
 namespace dai_nodes {
 
-Sync::Sync(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline)
-    : BaseNode(daiNodeName, node, pipeline) {
+Sync::Sync(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr<dai::Pipeline> pipeline) : BaseNode(daiNodeName, node, pipeline) {
     syncNode = pipeline->create<dai::node::Sync>();
     paramHandler = std::make_unique<param_handlers::SyncParamHandler>(node, daiNodeName);
     paramHandler->declareParams(syncNode);

depthai_ros_driver/src/param_handlers/camera_param_handler.cpp

@@ -34,7 +34,7 @@ void CameraParamHandler::declareParams() {
     declareAndLogParam<std::string>("i_external_calibration_path", "");
     declareAndLogParam("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200));
     declareAndLogParam("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500));
-	declareAndLogParam<bool>("i_restart_on_diagnostics_error", false);
+    declareAndLogParam<bool>("i_restart_on_diagnostics_error", false);
     declareAndLogParam<bool>("i_rs_compat", false);
     declareAndLogParam<bool>("i_restart_on_diagnostics_error", false);
     declareAndLogParam<bool>("i_publish_tf_from_calibration", false);

depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp

@@ -49,7 +49,7 @@ void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, d
 
 StereoParamHandler::~StereoParamHandler() = default;
 void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> stereo) {
-	    declareAndLogParam<int>("i_max_q_size", 30);
+    declareAndLogParam<int>("i_max_q_size", 30);
     declareAndLogParam<bool>("i_low_bandwidth", false);
     declareAndLogParam<int>("i_low_bandwidth_quality", 50);
     declareAndLogParam<int>("i_low_bandwidth_profile", 4);
@@ -117,7 +117,7 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
     height = declareAndLogParam<int>("i_height", height);
     stereo->setOutputSize(width, height);
     stereo->setDefaultProfilePreset(depthPresetMap.at(declareAndLogParam<std::string>("i_depth_preset", "HIGH_ACCURACY")));
-    if(declareAndLogParam<bool>("i_enable_distortion_correction", true)) {
+    if(declareAndLogParam<bool>("i_enable_distortion_correction", false)) {
         stereo->enableDistortionCorrection(true);
     }
     if(declareAndLogParam<bool>("i_set_disparity_to_depth_use_spec_translation", false)) {

depthai_ros_driver/src/utils.cpp

@@ -1,9 +1,8 @@
 #include "depthai_ros_driver/utils.hpp"
 
-#include "depthai_ros_driver/utils.hpp"
-
 #include "depthai/pipeline/Pipeline.hpp"
 #include "depthai/pipeline/node/XLinkOut.hpp"
+#include "depthai_ros_driver/utils.hpp"
 namespace depthai_ros_driver {
 namespace utils {
 std::string getUpperCaseStr(const std::string& string) {