4 Replies Latest reply on Apr 10, 2018 9:28 AM by Coline

    Publishing images from bottom camera to ROS topics

    Trixie

      Hi,

       

      Like the title says, I'm trying to publish images from the bottom camera of the RTF drone using ROS. Initially, I modified the existing usb_cam ROS package and this allowed me to communicate with the camera, but I've been struggling with trying to then calibrate my camera since the video stream that I get seems to just be a noisy image (I attached an image of it). To clarify, I'm still able to get some kind of stream but it seems to splice together 5 of the same streams into one, which can be seen with the 5 different columns.

       

      I've considered that the problem is simply a matter of decoding the yuv420 pixel format of the bottom camera into RGB, but I am still fairly new to C++ so I have been struggling with this problem. Am I going about this in the right direction or is something else causing this noisy stream?

       

      Any help would be appreciated.

        • 1. Re: Publishing images from bottom camera to ROS topics
          Trixie

          Actually, nevermind. My problem wasn't about the pixel format but when I was initializing the device.

          • 2. Re: Publishing images from bottom camera to ROS topics
            valts

            Would you mind sharing what you did to get it to work? I'm trying to do the same thing and I've no clue where to begin. Thanks!

            • 3. Re: Publishing images from bottom camera to ROS topics
              Trixie

              Apologies @valts, I didn't see your message sooner. What I did was modify the usb_cam ROS package like what I said in my original post. I had to change the camera driver to be able to specify the cameraId since the VGA and 8MP cameras both use the /dev/video2 device node. The function I changed was the init_device method in usb_cam.cpp and I ended up with this:

               

              void UsbCam::init_device(int image_width, int image_height, int framerate)
              {
                struct v4l2_capability cap;
                struct v4l2_cropcap cropcap;
                struct v4l2_crop crop;
                struct v4l2_format fmt;
                unsigned int min;
              
                struct v4l2_streamparm stream_params;
              
                if (-1 == xioctl(fd_, VIDIOC_S_INPUT, &cameraId))
                 errno_exit("VIDIOC_S_INPUT");
              
                memset(&stream_params, 0, sizeof(stream_params));
              
                stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
              
                stream_params.parm.capture.capturemode = 0x8000;
              
                if (-1 == xioctl(fd_, VIDIOC_S_PARM, &stream_params))
                 errno_exit("VIDIOC_S_PARM");
              
                if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
                 if (EINVAL == errno) {
                   ROS_ERROR_STREAM(camera_dev_ << " is no V4L2 device");
                   exit(EXIT_FAILURE);
                 } else {
                   errno_exit("VIDIOC_QUERYCAP");
                 }
                }
              
                if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
                {
                 ROS_ERROR_STREAM(camera_dev_ << " is no video capture device");
                 exit(EXIT_FAILURE);
                }
              
                switch (io_)
                {
                 case IO_METHOD_READ:
                   if (!(cap.capabilities & V4L2_CAP_READWRITE))  
                   {
                     ROS_ERROR_STREAM(camera_dev_ << " does not support read i/o");
                     exit(EXIT_FAILURE);
                   }
              
                   break;
              
                 case IO_METHOD_MMAP:
                 case IO_METHOD_USERPTR:
                   if (!(cap.capabilities & V4L2_CAP_STREAMING))
                   {
                     ROS_ERROR_STREAM(camera_dev_ << " does not support streaming i/o");
                     exit(EXIT_FAILURE);
                   }
              
                   break;
                }
              
                /* Select video input, video standard and tune here. */
              
                CLEAR(cropcap);
              
                cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
              
                if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap))
                {
                 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                 crop.c = cropcap.defrect; /* reset to default */
              
                 if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop))
                 {
                   switch (errno)
                   {
                     case EINVAL:
                     /* Cropping not supported. */
                       break;    
                     default:  
                     /* Errors ignored. */
                       break;
                   }  
                 }
                }
                else
                {
                 /* Errors ignored. */
                }
              
                CLEAR(fmt);
              
                fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                fmt.fmt.pix.width = image_width;
                fmt.fmt.pix.height = image_height;
                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
                fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
              
                if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt))
                 errno_exit("VIDIOC_S_FMT");
              
                image_width = fmt.fmt.pix.width;
                image_height = fmt.fmt.pix.height;
              
                if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0)
                 errno_exit("Couldn't query v4l fps!");
              
                ROS_DEBUG("Capability flag: 0x%x", stream_params.parm.capture.capability);
              
                stream_params.parm.capture.timeperframe.numerator = 1;
                stream_params.parm.capture.timeperframe.denominator = framerate;
                if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0)
                 ROS_WARN("Couldn't set camera framerate");
                else
                 ROS_DEBUG("Set framerate to be %i", framerate);
              
                switch (io_)
                {
                 case IO_METHOD_READ:
                   init_read(fmt.fmt.pix.sizeimage);
                   break;
              
                 case IO_METHOD_MMAP:
                   init_mmap();
                   break;
              
                 case IO_METHOD_USERPTR:
                   init_userp(fmt.fmt.pix.sizeimage);
                   break;
                }
              }
              

              Some parts of this might still be hardcoded since I haven't had time to clean it up properly.

               

              Best,

              Trixie

              1 of 1 people found this helpful
              • 4. Re: Publishing images from bottom camera to ROS topics
                Coline

                Thanks Trixie! Your code really helped me out. For anyone else trying to do the same thing the pixel format you need is 'grey' and set int cameraId=1.