Example on processing image before publishing in ROS2

mjunsen123

I am writing ROS2 publisher script in C++. Have anyone tried to process the image from auto imgQueue = device->getOutputQueue("rgb", 30, false);, before directly publish with

dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
            dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(
                imgQueue,
                node,
                std::string("color/image"),
                std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rgbConverter, std::placeholders::_1, std::placeholders::_2),
                30,
                rgbCameraInfo,
                "color");
            rgbPublish.addPublisherCallback();

Basically, i have bounding box from YoloDetectionNetwork, but instead of publish both of them, I would like to process the rgb / mono image with the bbox (i.e., drawing the bbox) and publish the processed frame. Can someone assist me with this?

jakaskerl

mjunsen123
Get both messages from the pipeline, combine them using opencv (draw the bounding box over the frame), then use that image in the publisher.

Thanks,
Jaka

mjunsen123

Thanks for the reply. So I do need to create my own publisher instead of using BridgePublisher?

jakaskerl

mjunsen123
Likely your own since the way the Publisher works is by taking in a queue; which you don't have after you have read both frames.

Thanks,
Jaka

mjunsen123

Thanks for that.

Another question:

[Scheduler00Thr] dispatcher Response Serve:928 no request for this response: XLINK_READ_REL_RESP 1

Sometime the camera crash with this error. May I know the root cause?

jakaskerl

mjunsen123
A general communication error, can't say more unfortunately.

Thanks,
Jaka

mjunsen123

So there is no workaround yet?

jakaskerl

mjunsen123
Where does the issue happen? Is it sporadic or can you replicate it?

Thanks
Jaka

mjunsen123

    // Setup IMU
    auto imu = pipeline.create<dai::node::IMU>();
    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
    imu->setBatchReportThreshold(5);
    imu->setMaxBatchReports(10); 

    // Setup left camera
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    monoLeft->setResolution(dai::node::MonoCamera::Properties::SensorResolution::THE_400_P);
    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
    monoLeft->setFps(20);

    // Setup right camera
    auto monoRight = pipeline.create<dai::node::MonoCamera>();
    monoRight->setResolution(dai::node::MonoCamera::Properties::SensorResolution::THE_400_P);
    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
    monoRight->setFps(20);

    // Setup depth camera
    auto stereo = pipeline.create<dai::node::StereoDepth>();
    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    stereo->setRectifyEdgeFillColor(0);                              
    stereo->setLeftRightCheck(true);
    stereo->setExtendedDisparity(false);
    stereo->setSubpixel(true);
    stereo->setDepthAlign(dai::CameraBoardSocket::CAM_B);

    // Setup Image Manip
    auto manip = pipeline.create<dai::node::ImageManip>();
    manip->initialConfig.setResizeThumbnail(W, H);
    manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p);

    // Setup YOLO detection network
    auto detectionNetwork = pipeline.create<dai::node::YoloDetectionNetwork>();
    detectionNetwork->setConfidenceThreshold(confidenceThreshold);
    detectionNetwork->setNumClasses(classes);
    detectionNetwork->setCoordinateSize(coordinates);
    detectionNetwork->setAnchors(anchors);
    detectionNetwork->setAnchorMasks(anchorMasks);
    detectionNetwork->setIouThreshold(iouThreshold);
    detectionNetwork->setBlobPath(nnPath);
    detectionNetwork->setNumInferenceThreads(2);
    detectionNetwork->setNumNCEPerInferenceThread(1);
    detectionNetwork->input.setBlocking(false);

    // Setup Sync node to sync all streams
    auto sync = pipeline.create<dai::node::Sync>();
    sync->setSyncThreshold(std::chrono::milliseconds(100));

    // Setup demux to seperate synced stream
    auto demux = pipeline.create<dai::node::MessageDemux>();

    // Define output streams
    auto xoutSyncedFrames = pipeline.create<dai::node::XLinkOut>();
    xoutSyncedFrames->setStreamName("frames");
    auto xoutSyncedDetections = pipeline.create<dai::node::XLinkOut>();
    xoutSyncedDetections->setStreamName("nn");
    auto xoutSyncedDepth = pipeline.create<dai::node::XLinkOut>();
    xoutSyncedDepth->setStreamName("depth");
    auto xoutImu = pipeline.create<dai::node::XLinkOut>();
    xoutImu->setStreamName("imu");

    // Linking
    monoLeft->out.link(stereo->left);
    monoRight->out.link(stereo->right);
    stereo->rectifiedLeft.link(manip->inputImage);
    stereo->rectifiedLeft.link(sync->inputs["rectifiedLeft"]);
    manip->out.link(detectionNetwork->input);
    detectionNetwork->out.link(sync->inputs["detections"]);
    stereo->depth.link(sync->inputs["depth"]);
    imu->out.link(xoutImu->input);
    sync->out.link(demux->input);
    demux->outputs["rectifiedLeft"].link(xoutSyncedFrames->input);
    demux->outputs["detections"].link(xoutSyncedDetections->input);
    demux->outputs["depth"].link(xoutSyncedDepth->input);

    return pipeline;

mjunsen123

*Sorry have trouble in using the message box.

Unfortunately I cannot replicate it but I have attach a pipeline for your reference and hope it helps

jakaskerl

mjunsen123
Does disabling the IMU help with the issue? We've had some similar issues before.

Thanks,
Jaka

mjunsen123

I am not sure whether disabling the IMU help with the issue. Sometimes it happen just 2-3 minutes i start the camera, sometime it happens after 10-20 min.