Hello - I am new to using the Oak-D camera. I want to use the camera with ROS. The camera is mounted upside down, so I need to flip the image. I can use rgb_publisher.cpp from the depthai-ros repo to incorporate the camera with ROS, but my camera image is upside down. I tried to incorporate code from ImageManip rotate as documented in this example to rotate my image 180 degrees, but my image then looks very goofy and fuzzy. Below is my code. Any idea what I'm doing wrong?

Thanks in advance!


#include <iostream>
#include <cstdio>
// #include "utility.hpp"
#include "sensor_msgs/Image.h"
#include <camera_info_manager/camera_info_manager.h>

// Inludes common necessary includes for development using depthai library
#include "depthai/depthai.hpp"
#include <depthai_bridge/BridgePublisher.hpp>
#include <depthai_bridge/ImageConverter.hpp>

dai::Pipeline createPipeline(){
    dai::Pipeline pipeline;
    auto colorCam = pipeline.create<dai::node::ColorCamera>();
        
    colorCam->setPreviewSize(300, 300);
    colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
    colorCam->setInterleaved(false);

    auto manipRgb = pipeline.create<dai::node::ImageManip>();
    dai::RotatedRect rgbRr = {{colorCam->getPreviewWidth() / 2.0f, colorCam->getPreviewHeight() / 2.0f},  // center
                              {colorCam->getPreviewHeight() * 1.0f, colorCam->getPreviewWidth() * 1.0f},  // size
                              180};                                                                    // angle
    manipRgb->initialConfig.setCropRotatedRect(rgbRr, false);
    colorCam->preview.link(manipRgb->inputImage);

    auto manipRgbOut = pipeline.create<dai::node::XLinkOut>();
    manipRgbOut->setStreamName("manip_rgb");
    manipRgb->out.link(manipRgbOut->input);

    // Link plugins CAM -> manipRgb 
    colorCam->video.link(manipRgbOut->input);
    return pipeline;
}

int main(int argc, char** argv){

    ros::init(argc, argv, "rgb_node");
    ros::NodeHandle pnh("~");
    
    std::string tfPrefix;
    std::string camera_param_uri;
    int badParams = 0;

    badParams += !pnh.getParam("tf_prefix", tfPrefix);
    badParams += !pnh.getParam("camera_param_uri", camera_param_uri);

    if (badParams > 0)
    {
        throw std::runtime_error("Couldn't find one of the parameters");
    }
    
    dai::Pipeline pipeline = createPipeline();
    dai::Device device(pipeline);
    //std::shared_ptr<dai::DataOutputQueue> imgQueue = device.getOutputQueue("manip_rgb", 30, false);
    std::shared_ptr<dai::DataOutputQueue> imgQueue = device.getOutputQueue("manip_rgb", 8, false);
    
    std::string color_uri = camera_param_uri + "/" + "color.yaml";

    dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
    dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> rgbPublish(imgQueue,
                                                                                  pnh, 
                                                                                  std::string("color/image"),
                                                                                  std::bind(&dai::rosBridge::ImageConverter::toRosMsg, 
                                                                                  &rgbConverter, // since the converter has the same frame name
                                                                                                  // and image type is also same we can reuse it
                                                                                  std::placeholders::_1, 
                                                                                  std::placeholders::_2) , 
                                                                                  30,
                                                                                  color_uri,
                                                                                  "color");

    rgbPublish.addPublisherCallback();
    ros::spin();

    return 0;
}

Sorry, putting code in my question didn't work as expected - the formatting is terrible. I don't find a way to edit the question to remove the code.

  • erik replied to this.

    Hello brh ,
    No worries, I just reformatted it. You need 3x ``` on each side🙂
    For rotating, I would try using ColorCamera::setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG). Could you try that out instead of ImageManip? I think with ImageManip it gets fuzzy as 300 isn't dividable by 16 (see limitation here). THoughts?
    Thanks, Erik

    Thanks, Erik. setImageOrientation() worked perfectly. My camera image is now flipped the correct way and the image looks great. Thanks much for pointing me in the right direction.

    6 months later

    Erik: Is there an easy way to do this in the ROS launch file controlling the camera?

    Never mind, I modified my previous work, using the one line above, and it works great! ha