How to configure CameraInfoManager set_camera_info service name?
How do I configure CameraInfoManager to publish the correct setcamerainfo services?
I am working on a very simple node to take stereo images produced by a stereo USB camera where the images come in as a single double-wide image consisting of left/right images concatenated. The node simply splits the incoming image down the middle and republishes on left and right image topics. I am trying to add the appropriate CameraInfoManager machinery to load/save calibration data. The current code is in the branch 'caminfo' at https://github.com/dbc/sidexsidestereo
I create two pointers to CamInfoManager instances:
// Camera info managers.
camera_info_manager::CameraInfoManager *left_cinfo_;
camera_info_manager::CameraInfoManager *right_cinfo_;
Allocate and init:
// Allocate and initialize camera info managers.
left_cinfo_ =
new camera_info_manager::CameraInfoManager(nh, "left_camera");
right_cinfo_ =
new camera_info_manager::CameraInfoManager(nh, "right_camera");
left_cinfo_->loadCameraInfo("");
right_cinfo_->loadCameraInfo("");
But I don't see 'leftcamera' or 'rightcamera' associated setcamerainfo services being advertised:
dave@ai1:~/catkin_ws$ rosservice list
/cameracalibrator/get_loggers
/cameracalibrator/set_logger_level
/image_raw/compressed/set_parameters
/image_raw/compressedDepth/set_parameters
/image_raw/theora/set_parameters
/libuvc_camera/get_loggers
/libuvc_camera/set_logger_level
/libuvc_camera/set_parameters
/rosout/get_loggers
/rosout/set_logger_level
/rqt_gui_cpp_node_5296/get_loggers
/rqt_gui_cpp_node_5296/set_logger_level
/rqt_gui_py_node_5296/get_loggers
/rqt_gui_py_node_5296/set_logger_level
/set_camera_info
/side_by_side_stereo_splitter_node/get_loggers
/side_by_side_stereo_splitter_node/set_camera_info
/side_by_side_stereo_splitter_node/set_logger_level
/stereo/left/image_raw/compressed/set_parameters
/stereo/left/image_raw/compressedDepth/set_parameters
/stereo/left/image_raw/theora/set_parameters
/stereo/right/image_raw/compressed/set_parameters
/stereo/right/image_raw/compressedDepth/set_parameters
/stereo/right/image_raw/theora/set_parameters
There is /sidebysidestereosplitternode/setcamera_info, but I am confused as to what is creating that.
So, I am clearly not setting up the CameraInfoManagers correctly to get the topics that camera_calibration expects. It is kind of whiney:
Traceback (most recent call last):
File "/opt/ros/melodic/lib/python2.7/dist-packages/camera_calibration/camera_calibrator.py", line 247, in on_mouse
if self.do_upload():
File "/opt/ros/melodic/lib/python2.7/dist-packages/camera_calibration/camera_calibrator.py", line 207, in do_upload
response = self.set_left_camera_info_service(info[0])
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 439, in __call__
return self.call(*args, **kwds)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 499, in call
service_uri = self._get_service_uri(request)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 467, in _get_service_uri
raise ServiceException("service [%s] unavailable"%self.resolved_name)
rospy.service.ServiceException: service [/left_camera/set_camera_info] unavailable
What do I need to do to get CameraInfoManager to advertise sensible services?
Asked by dcurtis on 2019-03-08 16:15:21 UTC
Answers
So this turns out to be a case where multiple node handles is the answer. With:
ros::NodeHandle nh("sxs_stereo");
ros::NodeHandle nh_left(nh, "left");
ros::NodeHandle nh_right(nh, "right");
...
left_cinfo_ =
new camera_info_manager::CameraInfoManager(nh_left);
right_cinfo_ =
new camera_info_manager::CameraInfoManager(nh_right);
The result is sensible service names, and the camera calibrator is happy.
Asked by dcurtis on 2019-03-11 17:10:52 UTC
Comments