ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

How to use camera_info_manager to publish camera_info

asked 2018-01-02 21:22:17 -0500

trixr4kdz gravatar image

updated 2018-01-03 21:41:23 -0500

I am trying to calibrate my ov7251 camera using the camera_calibration package. I adapted a sample camera driver that I found which use the UV4L API so that I can try to publish images to the ROS topic /camera/image. I am also publishing to /camera/camera_info. However, what I need now is to be able to run the /camera/set_camera_info service so that I can use the calibration package.

Here is my code for publishing to /camera/image and /camera/camera_info:

int main(int argc, char **argv) {
    dev_name = "/dev/video2";

    namedWindow( "Camera Preview", WINDOW_AUTOSIZE );// Create a window for display.
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1);

    ros::Publisher pub_info = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 100);
    sensor_msgs::CameraInfo cam_info;

    open_device();
    init_device();
    start_capturing();

    while (nh.ok()) {
        read_frame();
        if (!prev.empty()) {
            cv::Mat image = prev;
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

            ros::Rate loop_rate(5);

            cam_info.header.stamp = ros::Time::now();
            pub_info.publish(cam_info);
            pub.publish(msg);
            ros::spinOnce();
            loop_rate.sleep();
        }  
    }

   ...

   return 0;
}

When I ran rostopic list, this is what I get:

/camera/camera_info
/camera/image
/camera/image/compressed
/camera/image/compressed/parameter_descriptions
/camera/image/compressed/parameter_updates
/camera/image/compressedDepth
/camera/image/compressedDepth/parameter_descriptions
/camera/image/compressedDepth/parameter_updates
/camera/image/theora
/camera/image/theora/parameter_descriptions
/camera/image/theora/parameter_updates
/camera/image_raw
/rosout
/rosout_agg
/statistics
/svo/dense_input
/svo/image
/svo/image/compressed
/svo/image/compressed/parameter_descriptions
/svo/image/compressed/parameter_updates
/svo/image/compressedDepth
/svo/image/compressedDepth/parameter_descriptions
/svo/image/compressedDepth/parameter_updates
/svo/image/theora
/svo/image/theora/parameter_descriptions
/svo/image/theora/parameter_updates
/svo/info
/svo/keyframes
/svo/points
/svo/pose
/svo/remote_key
/tf

And when I run rosservice list:

/camera/image/compressed/set_parameters
/camera/image/compressedDepth/set_parameters
/camera/image/theora/set_parameters
/image_publisher/get_loggers
/image_publisher/set_logger_level
/rosout/get_loggers
/rosout/set_logger_level
/rqt_gui_py_node_13721/get_loggers
/rqt_gui_py_node_13721/set_logger_level
/rqt_gui_py_node_13807/get_loggers
/rqt_gui_py_node_13807/set_logger_level
/svo/get_loggers
/svo/image/compressed/set_parameters
/svo/image/compressedDepth/set_parameters
/svo/image/theora/set_parameters
/svo/set_logger_level

As you can see, I don't have a /set_camera_info service, which I think is the main problem I am having with trying to run the camera_calibration package. If anyone can please provide me with a simple sample code for using the camera_info_manager, it would be much appreciated.

EDIT: For now, I wish to simply initialize a camera_info_manager object so that I can use its functions to run the set_camera_info service, but I am not well-versed with C++ so I am having some problems understanding the tutorial for camera_info_manager.

EDIT2: I added these two lines in, based on @joq's comments:

boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo;
cinfo(new camera_info_manager::CameraInfoManager(nh));

but I only get this error:

no match for call to ‘(boost::shared_ptr<camera_info_manager::CameraInfoManager>) (camera_info_manager::CameraInfoManager*)’
cinfo(new camera_info_manager::CameraInfoManager(nh));

As a separate approach, I have also tried to simply do this to instantiate a camera info manager:

camera_info_manager::CameraInfoManager cinfo(nh, "camera","");

but then I get this error:

CMakeFiles/vga_cam_publisher.dir/src/vga_cam_publisher.cpp.o: In function ...
(more)
edit retag flag offensive close merge delete

Comments

Have you tried usb_cam package. It will automatically start the required service for camera calibration.

Ameer Hamza Khan gravatar image Ameer Hamza Khan  ( 2018-01-04 01:58:12 -0500 )edit

Yes, I have. But the main problem with the drivers that I looked through was that they did not give me the option to enter a "device_id", just the "device_name". Although I can set the param as "/dev/videoN", I think I need to be able to set the "device_id" to 0 or 1.

trixr4kdz gravatar image trixr4kdz  ( 2018-01-04 11:45:59 -0500 )edit

The error that I would get is [ERROR] [1515087853.012401358]: VIDIOC_S_FMT error 22, Invalid argument

trixr4kdz gravatar image trixr4kdz  ( 2018-01-04 11:46:43 -0500 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2018-01-03 10:59:57 -0500

joq gravatar image

updated 2018-01-03 11:03:51 -0500

Camera info manager will declare that service for you. The "tutorial" is just a link to the first driver that used camera_info_manager, not entirely self-explanatory.

To use camera_info_manager, you must:

There is not a lot more to it than that.

edit flag offensive delete link more

Comments

Thank you, and this might sound like a stupid question but why would we need to declare a pointer to an instance of ImageTransport? And would using Boost really be necessary?

I've also edited my original post to have the error I keep getting when I try to create a CameraInfoManager object

trixr4kdz gravatar image trixr4kdz  ( 2018-01-03 14:04:07 -0500 )edit

Boost pointers are not required, just useful for avoiding memory leaks. The main advantage of using a pointer is being able to control when the class is instantiated. Sometimes you need to process some ROS parameters first, and a static allocation would happen before then.

joq gravatar image joq  ( 2018-01-03 16:17:37 -0500 )edit

The error message looks like a CamelCase error in your boost pointer declaration, the class says camerainfomanager instead of CameraInfoManager.

joq gravatar image joq  ( 2018-01-03 16:27:21 -0500 )edit

Since you're using C++11, std::shared_ptr is built-in, so you don't need Boost.

joq gravatar image joq  ( 2018-01-03 16:28:15 -0500 )edit

Oh that's weird, the ROS forums text box is changing the case of CameraInfoManager to all lowercase. I do have the error as saying CameraInfoManager. I've made it into a code block here to correct it.

trixr4kdz gravatar image trixr4kdz  ( 2018-01-03 21:39:08 -0500 )edit

When I run catkin_make it seems to be able to link my file, but then I get the error about "undefined reference to `camera_info_manager::CameraInfoManager::CameraInfoManager(ros::NodeHandle, std::__cxx11::basic_string<char, std::char_traits<char="">, std::allocator<char> > const&, ...

trixr4kdz gravatar image trixr4kdz  ( 2018-01-03 23:22:10 -0500 )edit

Is camera_info_manager listed as a dependency in your package.xml?

joq gravatar image joq  ( 2018-01-04 15:11:05 -0500 )edit

It actually isn't but when I added it to the CMakeLists.txt file instead of the package.xml file, the error about undefined reference to CameraInfoManager disappeared. In any case, thank you for all the help. I will now do the rest of the steps you listed on your answer.

trixr4kdz gravatar image trixr4kdz  ( 2018-01-04 19:20:19 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-01-02 20:50:26 -0500

Seen: 6,121 times

Last updated: Jan 03 '18