The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
Hey thanks for getting back to me! I have implemented exactly what was suggested but I'm hitting a bit of a roadblock with the camera info. I used the opencv based camera driver found here (http://wiki.ros.org/video_stream_opencv) as my starting point. As you may know it publishes not only the image from the camera but also the camera information associated with a calibration. It seems that when you use more than one "camera_info_manager::CameraInfoManager" in the same place you get the following error message.
[ERROR] [1518120816.976389693]: Tried to advertise a service that is already advertised in this node [/camera/set_camera_info]
Does anyone have an idea how to get around this? You'll find my code and launch file below. I'm somewhat of a ROS newbie so if you happen to have more than one idea I'd love to hear the simplest one!
Also for reference here is the camera driver I found that does not work. http://wiki.ros.org/elp_stereo_camera
cpp file
/* * Software License Agreement (Modified BSD License) * * Copyright (c) 2016, PAL Robotics, S.L. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following *
disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of PAL Robotics, S.L. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * @author Sammy Pfeiffer */include <ros ros.h="">
include <image_transport image_transport.h="">
include <camera_info_manager camera_info_manager.h="">
include <opencv2 highgui="" highgui.hpp="">
include <opencv2 imgproc="" imgproc.hpp="">
include <cv_bridge cv_bridge.h="">
include <sstream>
include <boost assign="" list_of.hpp="">
// Based on the ros tutorial on transforming opencv images to Image messages
sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img){ sensor_msgs::CameraInfo cam_info_msg; cam_info_msg.header.frame_id = img->header.frame_id; // Fill image size cam_info_msg.height = img->height; cam_info_msg.width = img->width; ROS_INFO_STREAM("The image width is: " << img->width); ROS_INFO_STREAM("The image height is: " << img->height); // Add the most common distortion model as sensor_msgs/CameraInfo says cam_info_msg.distortion_model = "plumb_bob"; // Don't let distorsion matrix be empty cam_info_msg.D.resize(5, 0.0); // Give a reasonable default intrinsic camera matrix cam_info_msg.K = boost::assign::list_of(1.0) (0.0) (img->width/2.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (1.0); // Give a reasonable default rectification matrix cam_info_msg.R = boost::assign::list_of (1.0) (0.0) (0.0) (0.0) (1.0) (0.0) (0.0) (0.0) (1.0); // Give a reasonable default projection matrix cam_info_msg.P = boost::assign::list_of (1.0) (0.0) (img->width/2.0) (0.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (0.0) (1.0) (0.0); return cam_info_msg; }
int main(int argc, char** argv) { ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); // to get the private params image_transport::ImageTransport it(nh); image_transport::CameraPublisher pub_left = it.advertiseCamera("left", 1); image_transport::CameraPublisher pub_right = it.advertiseCamera("right", 1);
// provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number ofdevice, (e.g.: 0 would be /dev/video0) std::string video_stream_provider; cv::VideoCapture cap; if (_nh.getParam("video_stream_provider", video_stream_provider)){ ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider); // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected) // treat is as a number and act accordingly so we open up the videoNUMBER device if (video_stream_provider.size() < 4){ ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider); cap.open(atoi(video_stream_provider.c_str())); } else{ ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider); cap.open(video_stream_provider); } } else{ ROS_ERROR("Failed to get param 'video_stream_provider'"); return -1; }
std::string camera_name; _nh.param("camera_name", camera_name, std::string("camera")); ROS_INFO_STREAM("Camera name: " << camera_name); int fps; _nh.param("fps", fps, 240); ROS_INFO_STREAM("Throttling to fps: " << fps); std::string frame_id; _nh.param("frame_id", frame_id, std::string("camera")); ROS_INFO_STREAM("Publishing with frame_id: " << frame_id); std::string camera_info_url; _nh.param("camera_info_url", camera_info_url, std::string("")); ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url<< "'");
bool flip_horizontal; _nh.param("flip_horizontal", flip_horizontal, false); ROS_INFO_STREAM("Flip horizontal image is: " <<((flip_horizontal)?"true":"false"));
bool flip_vertical; _nh.param("flip_vertical", flip_vertical, false); ROS_INFO_STREAM("Flip vertical image is: " <<((flip_vertical)?"true":"false"));
int width_target; int height_target; _nh.param("width", width_target, 0); _nh.param("height", height_target, 0); if (width_target != 0 && height_target != 0){ ROS_INFO_STREAM("Forced image width is: " << width_target); ROS_INFO_STREAM("Forced image height is: " << height_target); } // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#voidflip(InputArray src, OutputArray dst, int flipCode) // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1 bool flip_image = true; int flip_value; if (flip_horizontal && flip_vertical) flip_value = 0; // flip both, horizontal and vertical else if (flip_horizontal) flip_value = 1; else if (flip_vertical) flip_value = -1; else flip_image = false;
if(!cap.isOpened()){ ROS_ERROR_STREAM("Could not open the stream."); return -1; } if (width_target != 0 && height_target != 0){ cap.set(CV_CAP_PROP_FRAME_WIDTH,width_target); cap.set(CV_CAP_PROP_FRAME_HEIGHT, height_target); }
ROS_INFO_STREAM("Opened the stream, starting to publish."); cv::Mat frame; sensor_msgs::ImagePtr msg_left; sensor_msgs::ImagePtr msg_right; sensor_msgs::CameraInfo cam_info_msg; sensor_msgs::CameraInfo cam_info_msg_2; std_msgs::Header header; header.frame_id = frame_id; camera_info_manager::CameraInfoManagercam_info_manager(nh,"elp_left", camera_info_url); // Get the saved camera info if any cam_info_msg = cam_info_manager.getCameraInfo();
camera_info_manager::CameraInfoManagercam_info_manager_2(nh, "elp_right", camera_info_url); // Get the saved camera info if any cam_info_msg_2 = cam_info_manager_2.getCameraInfo();
ros::Rate r(fps); while (nh.ok()) { cap >> frame;//check both publishers if (pub_left.getNumSubscribers() > 0 || pub_right.getNumSubscribers() > 0){
// Check if grabbed frame is actually full with some content if(!frame.empty()) { // Flip the image if necessary if (flip_image) cv::flip(frame, frame, flip_value); cv::Mat left_image=frame(cv::Rect(0,0,640,480)); cv::Matright_image=frame(cv::Rect(640,0,640,480));
msg_left = cv_bridge::CvImage(header, "bgr8",left_image).toImageMsg(); msg_right = cv_bridge::CvImage(header, "bgr8", right_image).toImageMsg();
// Create a default camera info if we didn't get a storedone on initialization if (cam_info_msg.distortion_model == ""){ ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info."); cam_info_msg = get_default_camera_info_from_image(msg_left); cam_info_manager.setCameraInfo(cam_info_msg); } // The timestamps are in sync thanks to this publisher ros::Time pub_time=ros::Time::now();
pub_right.publish(*msg_right,cam_info_msg, pub_time); pub_left.publish(*msg_left, cam_info_msg, pub_time);
} ros::spinOnce(); } r.sleep(); } }
| | 2 | No.2 Revision |
Hey thanks for getting back to me! I have implemented exactly what was suggested but I'm hitting a bit of a roadblock with the camera info. I used the opencv based camera driver found here (http://wiki.ros.org/video_stream_opencv) as my starting point. As you may know it publishes not only the image from the camera but also the camera information associated with a calibration. It seems that when you use more than one "camera_info_manager::CameraInfoManager" in the same place you get the following error message.
[ERROR] [1518120816.976389693]: Tried to advertise a service that is already advertised in this node [/camera/set_camera_info]
Does anyone have an idea how to get around this? You'll find my code and launch file below. I'm somewhat of a ROS newbie so if you happen to have more than one idea I'd love to hear the simplest one!
Also for reference here is the camera driver I found that does not work. http://wiki.ros.org/elp_stereo_camera
cpp file
/* * Software License Agreement (Modified BSD License) * * Copyright (c) 2016, PAL Robotics, S.L. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following *
disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of PAL Robotics, S.L. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * @author Sammy Pfeiffer */include <ros ros.h="">
include <image_transport image_transport.h="">
include <camera_info_manager camera_info_manager.h="">
include <opencv2 highgui="" highgui.hpp="">
include <opencv2 imgproc="" imgproc.hpp="">
include <cv_bridge cv_bridge.h="">
include <sstream>
include <boost assign="" list_of.hpp="">
// Based on the ros tutorial on transforming opencv images to Image messages
sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img){ sensor_msgs::CameraInfo cam_info_msg; cam_info_msg.header.frame_id = img->header.frame_id; // Fill image size cam_info_msg.height = img->height; cam_info_msg.width = img->width; ROS_INFO_STREAM("The image width is: " << img->width); ROS_INFO_STREAM("The image height is: " << img->height); // Add the most common distortion model as sensor_msgs/CameraInfo says cam_info_msg.distortion_model = "plumb_bob"; // Don't let distorsion matrix be empty cam_info_msg.D.resize(5, 0.0); // Give a reasonable default intrinsic camera matrix cam_info_msg.K = boost::assign::list_of(1.0) (0.0) (img->width/2.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (1.0); // Give a reasonable default rectification matrix cam_info_msg.R = boost::assign::list_of (1.0) (0.0) (0.0) (0.0) (1.0) (0.0) (0.0) (0.0) (1.0); // Give a reasonable default projection matrix cam_info_msg.P = boost::assign::list_of (1.0) (0.0) (img->width/2.0) (0.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (0.0) (1.0) (0.0); return cam_info_msg; }
int main(int argc, char** argv) { ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); // to get the private params image_transport::ImageTransport it(nh); image_transport::CameraPublisher pub_left = it.advertiseCamera("left", 1); image_transport::CameraPublisher pub_right = it.advertiseCamera("right", 1);
// provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number ofdevice, (e.g.: 0 would be /dev/video0) std::string video_stream_provider; cv::VideoCapture cap; if (_nh.getParam("video_stream_provider", video_stream_provider)){ ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider); // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected) // treat is as a number and act accordingly so we open up the videoNUMBER device if (video_stream_provider.size() < 4){ ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider); cap.open(atoi(video_stream_provider.c_str())); } else{ ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider); cap.open(video_stream_provider); } } else{ ROS_ERROR("Failed to get param 'video_stream_provider'"); return -1; }
std::string camera_name; _nh.param("camera_name", camera_name, std::string("camera")); ROS_INFO_STREAM("Camera name: " << camera_name); int fps; _nh.param("fps", fps, 240); ROS_INFO_STREAM("Throttling to fps: " << fps); std::string frame_id; _nh.param("frame_id", frame_id, std::string("camera")); ROS_INFO_STREAM("Publishing with frame_id: " << frame_id); std::string camera_info_url; _nh.param("camera_info_url", camera_info_url, std::string("")); ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url<< "'");
bool flip_horizontal; _nh.param("flip_horizontal", flip_horizontal, false); ROS_INFO_STREAM("Flip horizontal image is: " <<((flip_horizontal)?"true":"false"));
bool flip_vertical; _nh.param("flip_vertical", flip_vertical, false); ROS_INFO_STREAM("Flip vertical image is: " <<((flip_vertical)?"true":"false"));
int width_target; int height_target; _nh.param("width", width_target, 0); _nh.param("height", height_target, 0); if (width_target != 0 && height_target != 0){ ROS_INFO_STREAM("Forced image width is: " << width_target); ROS_INFO_STREAM("Forced image height is: " << height_target); } // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#voidflip(InputArray src, OutputArray dst, int flipCode) // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1 bool flip_image = true; int flip_value; if (flip_horizontal && flip_vertical) flip_value = 0; // flip both, horizontal and vertical else if (flip_horizontal) flip_value = 1; else if (flip_vertical) flip_value = -1; else flip_image = false;
if(!cap.isOpened()){ ROS_ERROR_STREAM("Could not open the stream."); return -1; } if (width_target != 0 && height_target != 0){ cap.set(CV_CAP_PROP_FRAME_WIDTH,width_target); cap.set(CV_CAP_PROP_FRAME_HEIGHT, height_target); }
ROS_INFO_STREAM("Opened the stream, starting to publish."); cv::Mat frame; sensor_msgs::ImagePtr msg_left; sensor_msgs::ImagePtr msg_right; sensor_msgs::CameraInfo cam_info_msg; sensor_msgs::CameraInfo cam_info_msg_2; std_msgs::Header header; header.frame_id = frame_id; camera_info_manager::CameraInfoManagercam_info_manager(nh,"elp_left", camera_info_url); // Get the saved camera info if any cam_info_msg = cam_info_manager.getCameraInfo();
camera_info_manager::CameraInfoManagercam_info_manager_2(nh, "elp_right", camera_info_url); // Get the saved camera info if any cam_info_msg_2 = cam_info_manager_2.getCameraInfo();
ros::Rate r(fps); while (nh.ok()) { cap >> frame;//check both publishers if (pub_left.getNumSubscribers() > 0 || pub_right.getNumSubscribers() > 0){
// Check if grabbed frame is actually full with some content if(!frame.empty()) { // Flip the image if necessary if (flip_image) cv::flip(frame, frame, flip_value); cv::Mat left_image=frame(cv::Rect(0,0,640,480)); cv::Matright_image=frame(cv::Rect(640,0,640,480));
msg_left = cv_bridge::CvImage(header, "bgr8",left_image).toImageMsg(); msg_right = cv_bridge::CvImage(header, "bgr8", right_image).toImageMsg();
// Create a default camera info if we didn't get a storedone on initialization if (cam_info_msg.distortion_model == ""){ ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info."); cam_info_msg = get_default_camera_info_from_image(msg_left); cam_info_manager.setCameraInfo(cam_info_msg); } // The timestamps are in sync thanks to this publisher ros::Time pub_time=ros::Time::now();
pub_right.publish(*msg_right,cam_info_msg, pub_time); pub_left.publish(*msg_left, cam_info_msg, pub_time);
} ros::spinOnce(); } r.sleep(); } }
| | 3 | No.3 Revision |
Hey thanks for getting back to me! I have implemented exactly what was suggested but I'm hitting a bit of a roadblock with the camera info. I used the opencv based camera driver found here (http://wiki.ros.org/video_stream_opencv) as my starting point. As you may know it publishes not only the image from the camera but also the camera information associated with a calibration. It seems that when you use more than one "camera_info_manager::CameraInfoManager" in the same place you get the following error message.
[ERROR] [1518120816.976389693]: Tried to advertise a service that is already advertised in this node [/camera/set_camera_info]
Does anyone have an idea how to get around this? You'll find my code and launch file below. I'm somewhat of a ROS newbie so if you happen to have more than one idea I'd love to hear the simplest one!
Also for reference here is the camera driver I found that does not work. http://wiki.ros.org/elp_stereo_camera
cpp file
include <ros ros.h="">
include <image_transport image_transport.h="">
include <camera_info_manager camera_info_manager.h="">
include <opencv2 highgui="" highgui.hpp="">
include <opencv2 imgproc="" imgproc.hpp="">
include <cv_bridge cv_bridge.h="">
include <sstream>
include <boost assign="" list_of.hpp="">
// Based on the ros tutorial on transforming opencv images to Image messages
sensor_msgs::CameraInfo get_default_camera_info_from_image(sensor_msgs::ImagePtr img){ sensor_msgs::CameraInfo cam_info_msg; cam_info_msg.header.frame_id = img->header.frame_id; // Fill image size cam_info_msg.height = img->height; cam_info_msg.width = img->width; ROS_INFO_STREAM("The image width is: " << img->width); ROS_INFO_STREAM("The image height is: " << img->height); // Add the most common distortion model as sensor_msgs/CameraInfo says cam_info_msg.distortion_model = "plumb_bob"; // Don't let distorsion matrix be empty cam_info_msg.D.resize(5, 0.0); // Give a reasonable default intrinsic camera matrix cam_info_msg.K = boost::assign::list_of(1.0) (0.0) (img->width/2.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (1.0); // Give a reasonable default rectification matrix cam_info_msg.R = boost::assign::list_of (1.0) (0.0) (0.0) (0.0) (1.0) (0.0) (0.0) (0.0) (1.0); // Give a reasonable default projection matrix cam_info_msg.P = boost::assign::list_of (1.0) (0.0) (img->width/2.0) (0.0) (0.0) (1.0) (img->height/2.0) (0.0) (0.0) (0.0) (1.0) (0.0); return cam_info_msg; }
int main(int argc, char** argv) { ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); // to get the private params image_transport::ImageTransport it(nh); image_transport::CameraPublisher pub_left = it.advertiseCamera("left", 1); image_transport::CameraPublisher pub_right = it.advertiseCamera("right", 1);
// provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number ofdevice, (e.g.: 0 would be /dev/video0) std::string video_stream_provider; cv::VideoCapture cap; if (_nh.getParam("video_stream_provider", video_stream_provider)){ ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider); // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected) // treat is as a number and act accordingly so we open up the videoNUMBER device if (video_stream_provider.size() < 4){ ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider); cap.open(atoi(video_stream_provider.c_str())); } else{ ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider); cap.open(video_stream_provider); } } else{ ROS_ERROR("Failed to get param 'video_stream_provider'"); return -1; }
std::string camera_name; _nh.param("camera_name", camera_name, std::string("camera")); ROS_INFO_STREAM("Camera name: " << camera_name); int fps; _nh.param("fps", fps, 240); ROS_INFO_STREAM("Throttling to fps: " << fps); std::string frame_id; _nh.param("frame_id", frame_id, std::string("camera")); ROS_INFO_STREAM("Publishing with frame_id: " << frame_id); std::string camera_info_url; _nh.param("camera_info_url", camera_info_url, std::string("")); ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url<< "'");
bool flip_horizontal; _nh.param("flip_horizontal", flip_horizontal, false); ROS_INFO_STREAM("Flip horizontal image is: " <<((flip_horizontal)?"true":"false"));
bool flip_vertical; _nh.param("flip_vertical", flip_vertical, false); ROS_INFO_STREAM("Flip vertical image is: " <<((flip_vertical)?"true":"false"));
int width_target; int height_target; _nh.param("width", width_target, 0); _nh.param("height", height_target, 0); if (width_target != 0 && height_target != 0){ ROS_INFO_STREAM("Forced image width is: " << width_target); ROS_INFO_STREAM("Forced image height is: " << height_target); } // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#voidflip(InputArray src, OutputArray dst, int flipCode) // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1 bool flip_image = true; int flip_value; if (flip_horizontal && flip_vertical) flip_value = 0; // flip both, horizontal and vertical else if (flip_horizontal) flip_value = 1; else if (flip_vertical) flip_value = -1; else flip_image = false;
if(!cap.isOpened()){ ROS_ERROR_STREAM("Could not open the stream."); return -1; } if (width_target != 0 && height_target != 0){ cap.set(CV_CAP_PROP_FRAME_WIDTH,width_target); cap.set(CV_CAP_PROP_FRAME_HEIGHT, height_target); }
ROS_INFO_STREAM("Opened the stream, starting to publish."); cv::Mat frame; sensor_msgs::ImagePtr msg_left; sensor_msgs::ImagePtr msg_right; sensor_msgs::CameraInfo cam_info_msg; sensor_msgs::CameraInfo cam_info_msg_2; std_msgs::Header header; header.frame_id = frame_id; camera_info_manager::CameraInfoManagercam_info_manager(nh,"elp_left", camera_info_url); // Get the saved camera info if any cam_info_msg = cam_info_manager.getCameraInfo();
camera_info_manager::CameraInfoManagercam_info_manager_2(nh, "elp_right", camera_info_url); // Get the saved camera info if any cam_info_msg_2 = cam_info_manager_2.getCameraInfo();
ros::Rate r(fps); while (nh.ok()) { cap >> frame;//check both publishers if (pub_left.getNumSubscribers() > 0 || pub_right.getNumSubscribers() > 0){
// Check if grabbed frame is actually full with some content if(!frame.empty()) { // Flip the image if necessary if (flip_image) cv::flip(frame, frame, flip_value); cv::Mat left_image=frame(cv::Rect(0,0,640,480)); cv::Matright_image=frame(cv::Rect(640,0,640,480));
msg_left = cv_bridge::CvImage(header, "bgr8",left_image).toImageMsg(); msg_right = cv_bridge::CvImage(header, "bgr8", right_image).toImageMsg();
// Create a default camera info if we didn't get a storedone on initialization if (cam_info_msg.distortion_model == ""){ ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info."); cam_info_msg = get_default_camera_info_from_image(msg_left); cam_info_manager.setCameraInfo(cam_info_msg); } // The timestamps are in sync thanks to this publisher ros::Time pub_time=ros::Time::now();
pub_right.publish(*msg_right,cam_info_msg, pub_time); pub_left.publish(*msg_left, cam_info_msg, pub_time);
} ros::spinOnce(); } r.sleep(); } }
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
