双目相机发布并显示ROS图像话题topic
双目相机发布ROS图像消息代码
- 概要
- 双ID双目相机消息发布代码
- 单ID双目相机消息发布代码
- 接受ros消息并显示图像信息的ros代码
概要
提供双目相机发布ROS话题代码 by十里桃园
发布的话题为：/shilitaoyuan_left 、 /shilitaoyuan_right
发布代码提供两份：一为相机只有一个ID，二是两个独立的相机有两个相机ID。
并提供接受显示图像信息的ros代码
可被应用于VINS双目等开源软件
双ID双目相机消息发布代码
#include
#include
#include
#include
#include
#include
using namespace cv;
int main(int argc, char** argv) {
ros::init(argc, argv, "image_publisher");
//ros::Time time_0= ros::Time::now ();
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
// image_transport::Publisher pub = it.advertise("/leftImage", 1);
//image_transport::Publisher pub1 = it.advertise("/rightImage", 1);
image_transport::Publisher pub = it.advertise("shilitaoyuan_/left", 1);
image_transport::Publisher pub1 = it.advertise("shilitaoyuan_/right", 1);
image_transport::Publisher pub0 = it.advertise("shilitaoyuan_/full", 1);
cv::VideoCapture cap1(0);
cap1.set(CV_CAP_PROP_FRAME_WIDTH,640);
cap1.set(CV_CAP_PROP_FRAME_HEIGHT,480);
cap1.set(CV_CAP_PROP_FPS, 30);
cv::VideoCapture cap2(1);
cap2.set(CV_CAP_PROP_FRAME_WIDTH,640);
cap2.set(CV_CAP_PROP_FRAME_HEIGHT,480);
cap2.set(CV_CAP_PROP_FPS, 30);
// cv::VideoCapture cap1(0);
if (!cap1.isOpened()) {
ROS_INFO("cannot open video device0\n");
return 1;
}
if (!cap2.isOpened()) {
ROS_INFO("cannot open video device0\n");
return 1;
}
// if (!cap1.isOpened()) {
// ROS_INFO("cannot open video device1\n");
// return 1;
// }
sensor_msgs::ImagePtr msg;
sensor_msgs::ImagePtr msg1;
sensor_msgs::ImagePtr msg0;
ros::Rate loop_rate(10);//hz
cv::Mat frameR;
cv::Mat frameL;
//cv::Mat frame;
// cv::Mat frameG;
//cap >> frame;
while (nh.ok()) {
cap2 >> frameR;
ros::Time time_R= ros::Time::now ();
cap1 >> frameL;
ros::Time time_L= ros::Time::now ();
//cvtColor(frame,frameG, COLOR_BGR2GRAY);
//std::cout << "frame "<<"frame.cols: "<header.stamp = time_R;
pub.publish(msg);
}
if (!frameL.empty()) {
msg1 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frameL).toImageMsg();
msg1->header.stamp = time_L;
pub1.publish(msg1);
}
//if (!frame.empty()) {
// msg0 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
// msg0->header.stamp = time_;
// pub0.publish(msg0);
// }
ROS_INFO("Publishing shilitaoyuan_/right shilitaoyuan_/left shilitaoyuan_/full ROS topic MSG!! ");
ros::spinOnce();
loop_rate.sleep();//与ros::Rate loop_rate相对应
}
}
单ID双目相机消息发布代码
#include
#include
#include
#include
#include
#include
using namespace cv;
int main(int argc, char** argv) {
ros::init(argc, argv, "image_publisher");
//ros::Time time_0= ros::Time::now ();
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
// image_transport::Publisher pub = it.advertise("/leftImage", 1);
//image_transport::Publisher pub1 = it.advertise("/rightImage", 1);
image_transport::Publisher pub = it.advertise("shilitaoyuan_/left", 1);
image_transport::Publisher pub1 = it.advertise("shilitaoyuan_/right", 1);
image_transport::Publisher pub0 = it.advertise("shilitaoyuan_/full", 1);
cv::VideoCapture cap(0);
cap.set(CV_CAP_PROP_FRAME_WIDTH,1280);
cap.set(CV_CAP_PROP_FRAME_HEIGHT,480);
cap.set(CV_CAP_PROP_FPS, 30);
// cv::VideoCapture cap1(0);
if (!cap.isOpened()) {
ROS_INFO("cannot open video device0\n");
return 1;
}
// if (!cap1.isOpened()) {
// ROS_INFO("cannot open video device1\n");
// return 1;
// }
sensor_msgs::ImagePtr msg;
sensor_msgs::ImagePtr msg1;
sensor_msgs::ImagePtr msg0;
ros::Rate loop_rate(30);//hz
cv::Mat frameR;
cv::Mat frameL;
cv::Mat frame;
cv::Mat frameG;
//cap >> frame;
while (nh.ok()) {
cap >> frame;
ros::Time time_= ros::Time::now ();
//cvtColor(frame,frameG, COLOR_BGR2GRAY);
std::cout << "frame "<<"frame.cols: "<header.stamp = time_;
pub.publish(msg);
}
if (!frameL.empty()) {
msg1 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frameL).toImageMsg();
msg1->header.stamp = time_;
pub1.publish(msg1);
}
if (!frame.empty()) {
msg0 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
msg0->header.stamp = time_;
pub0.publish(msg0);
}
ROS_INFO("Publishing shilitaoyuan_/right shilitaoyuan_/left shilitaoyuan_/full ROS topic MSG!! ");
ros::spinOnce();
loop_rate.sleep();//与ros::Rate loop_rate相对应
}
}
接受ros消息并显示图像信息的ros代码
#include
#include
#include
#include
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
try
{
cv::imshow("shilitaoyuan_right", cv_bridge::toCvShare(msg, "bgr8")->image);
cv::waitKey(1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
void imageCallback1(const sensor_msgs::ImageConstPtr& msg)
{
try
{
cv::imshow("shilitaoyuan_left", cv_bridge::toCvShare(msg, "bgr8")->image);
cv::waitKey(1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
void imageCallback0(const sensor_msgs::ImageConstPtr& msg)
{
try
{
cv::imshow("shilitaoyuan_full", cv_bridge::toCvShare(msg, "bgr8")->image);
cv::waitKey(1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_listener");
ros::NodeHandle nh;
cv::namedWindow("right");
cv::namedWindow("left");
cv::startWindowThread();
image_transport::ImageTransport it(nh);
// image_transport::Subscriber sub = it.subscribe("shilitaoyuan_/rightImage", 1, imageCallback);
image_transport::Subscriber sub = it.subscribe("shilitaoyuan_/right", 1, imageCallback);
image_transport::ImageTransport it1(nh);
image_transport::Subscriber sub1 = it.subscribe("shilitaoyuan_/left", 1, imageCallback1);
image_transport::ImageTransport it0(nh);
image_transport::Subscriber sub0 = it.subscribe("shilitaoyuan_/full", 1, imageCallback0);
// image_transport::Subscriber sub1 = it.subscribe("shilitaoyuan_/leftImage", 1, imageCallback1);
ros::spin();
cv::destroyWindow("shilitaoyuan_right");
cv::destroyWindow("shilitaoyuan_left");
}