双目相机发布并显示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");
}

你可能感兴趣的:(SLAM)