1 2 Previous Next 27 Replies Latest reply on Sep 6, 2018 10:36 AM by AlexisTM

    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

              3 of 3 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.

                • 5. Re: Publishing images from bottom camera to ROS topics
                  shakeeb

                  Thank you Trixie and Coline . This discussion really helped me. I have been trying to do the similar thing but somehow I am having problems while setting the camera frame rate . I am getting

                   

                  $ roslaunch usb_cam usb_cam.launch

                  ... logging to /home/aero/.ros/log/15cc9aa0-48a1-11e8-8bc5-72689d3f106b/roslaunch-aero-8057.log

                  Checking log directory for disk usage. This may take awhile.

                  Press Ctrl-C to interrupt

                  Done checking log file disk usage. Usage is <1GB.

                   

                  started roslaunch server http://aero:33698/

                   

                  SUMMARY

                  ========

                   

                  PARAMETERS

                  * /rosdistro: kinetic

                  * /rosversion: 1.12.13

                  * /usb_cam/camera_frame_id: usb_cam

                  * /usb_cam/image_height: 480

                  * /usb_cam/image_width: 640

                  * /usb_cam/io_method: mmap

                  * /usb_cam/pixel_format: grey

                  * /usb_cam/video_device: /dev/video2

                   

                  NODES

                    /

                      usb_cam (usb_cam/usb_cam_node)

                   

                  ROS_MASTER_URI=http://localhost:11311

                   

                  process[usb_cam-1]: started with pid [8074]

                  [ INFO] [1524673020.178612270]: using default calibration URL

                  [ INFO] [1524673020.178778798]: camera calibration URL: file:///home/aero/.ros/camera_info/head_camera.yaml

                  [ INFO] [1524673020.178932426]: Unable to open camera calibration file [/home/aero/.ros/camera_info/head_camera.yaml]

                  [ WARN] [1524673020.178994965]: Camera calibration file /home/aero/.ros/camera_info/head_camera.yaml not found.

                  [ INFO] [1524673020.179062853]: Starting 'head_camera' (/dev/video2) at 640x480 via mmap (grey) at 30 FPS

                  [ WARN] [1524673020.299492649]: Couldn't set camera framerate

                  [ WARN] [1524673020.311671785]: Failed to open /dev/video2: Device or resource busy

                   

                  [ WARN] [1524673020.319405389]: Failed to open /dev/video2: Device or resource busy

                   

                  Can anyone help with this or I am missing something completely? Thank you

                  • 6. Re: Publishing images from bottom camera to ROS topics
                    Coline

                    Hi shakeeb,

                     

                    I get similar warnings but I can still stream with image_view

                    Here is the output I get for reference

                    SUMMARY

                    ========

                     

                    PARAMETERS

                    * /image_view/autosize: True

                    * /rosdistro: kinetic

                    * /rosversion: 1.12.13

                    * /usb_cam/camera_frame_id: usb_cam

                    * /usb_cam/framerate: 30

                    * /usb_cam/image_height: 480

                    * /usb_cam/image_width: 640

                    * /usb_cam/io_method: userptr

                    * /usb_cam/pixel_format: grey

                    * /usb_cam/video_device: /dev/video2

                     

                    NODES

                      /

                        image_view (image_view/image_view)

                        usb_cam (usb_cam/usb_cam_node)

                     

                    auto-starting new master

                    process[master]: started with pid [2439]

                    ROS_MASTER_URI=http://localhost:11311

                     

                    setting /run_id to dc8070c0-4245-11e8-aa35-3200479c7b50

                    process[rosout-1]: started with pid [2452]

                    started core service [/rosout]

                    process[usb_cam-2]: started with pid [2455]

                    process[image_view-3]: started with pid [2456]

                    [ INFO] [1523972846.936222314]: using default calibration URL

                    [ INFO] [1523972846.936394332]: camera calibration URL: file:///home/coline/.ros/camera_info/head_camera.yaml

                    [ INFO] [1523972846.936559812]: Unable to open camera calibration file [/home/coline/.ros/camera_info/head_camera.yaml]

                    [ WARN] [1523972846.936612114]: Camera calibration file /home/coline/.ros/camera_info/head_camera.yaml not found.

                    [ INFO] [1523972846.936672041]: Starting 'head_camera' (/dev/video2) at 640x480 via userptr (grey) at 30 FPS

                    [ WARN] [1523972847.058261827]: Couldn't set camera framerate

                    [ INFO] [1523972847.065940069]: Using transport "raw"

                    [ WARN] [1523972847.081353066]: Failed to open /dev/video2: Device or resource busy

                     

                    [ WARN] [1523972847.093548857]: Failed to open /dev/video2: Device or resource busy

                    • 7. Re: Publishing images from bottom camera to ROS topics
                      chillax

                      Thank you Trixie and Coline for your contribution. Helped me out! shakeeb you need to change the io_method to userptr

                       

                      For those struggling with the same problem here is a quick guide to have the bottom VGA camera as a ROS topic:

                       

                      cd ~/catkin_ws/src #cd into the src folder of your catkin workspace

                      git clone https://github.com/szebedy/usb_cam.git

                      cd ..

                      catkin_make

                      source ./devel/setup.bash

                      roslaunch usb_cam usb_cam.launch /

                      rosrun image_view image_view image:=/usb_cam/image_raw

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

                        Hi chillax

                         

                        Thanks for your sharing.It's really useful.

                        I have some question.

                        Actually I got the same situation like Coline

                        I want know if there is any way I can get the video or picture for calibration the camera?

                         

                        Thanks

                        • 9. Re: Publishing images from bottom camera to ROS topics
                          chillax

                          Hi tuantuan91,

                           

                          I'm not sure if I understood your question but if you followed my instructions you should have an image stream from the bottom camera available under the /usb_cam/image_raw ROS topic. You can use the images on this topic to calibrate your camera.

                          • 10. Re: Publishing images from bottom camera to ROS topics
                            Coline

                            Hi tuantuan91,

                             

                            I calibrated the camera using the camera calibration tool from camera_calibration - ROS Wiki (it was already included in my ROS desktop full install).

                            It's pretty straightforward if you follow camera_calibration/Tutorials/MonocularCalibration - ROS Wiki

                             

                            Hope that helps

                            • 11. Re: Publishing images from bottom camera to ROS topics
                              tuantuan91

                              Hi chillax

                               

                              I just do as your instructions above.

                              The topic is available but there  is no images output.

                              I can get the message (grey value between 0-255) through the command: rostopic echo.

                              Is there any procedure I missed?

                               

                              Thanks

                              • 12. Re: Publishing images from bottom camera to ROS topics
                                chillax

                                Hi tuantuan91

                                 

                                rosrun image_view image_view image:=/usb_cam/image_raw is the answer you seek.

                                 

                                Have fun

                                • 13. Re: Publishing images from bottom camera to ROS topics
                                  tuantuan91

                                  Hi chillax

                                   

                                  It works well!

                                  Thanks for your help.

                                  I'm wondering if you are using the RTF to do the SLAM/VO research?

                                  • 14. Re: Publishing images from bottom camera to ROS topics
                                    tuantuan91

                                    Hi chillax

                                     

                                    Sorry to disturb you again.

                                    Did you calibrate the R200 camera use the same way?

                                     

                                    Thanks

                                    1 2 Previous Next