gazebo中仿真双目相机(stereo camera)

注：在gazebo中为px4添加相机，可以参考这篇 博客

1、仿真环境

仿真环境为 ROS-Academy-for-Beginners

环境搭建过程参考：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter1/1.5.html

2、双目相机 sdf文件

  
      
      
        true
        10
        false
        
          0 0 0 0 0 0
          1.047
          
            320
            240
          
          
            0.1
            100
          
        
        
          0 -0.07 0 0 0 0
          1.047
          
            320
            240
          
          
            0.1
            100
          
                
        
          stereocamera
          true
          10
          stereocamera
          image_raw
          camera_info
          camera_link_optical
          0.07
          0.0
          0.0
          0.0
          0.0
          0.0
        
      
    

3 、将双目相机添加到仿真机器人xbot-u中

找到 ROS-Academy-for-Beginners/robot_sim_demo/urdf 中 xbot-u.gazebo文件

 将双目相机代码添加到xbot-u.gazebo文件中

 4、编写程序读取双目相机的左右图

#include   
#include //ROS图象类型的编码函数
#include //用来在ROS系统中的话题上发布和订阅图象消息

//OpenCV2标准头文件
#include
#include
#include
#include     //cv_bridge中包含CvBridge库

#include //C++标准输入输出库
using namespace std;

//消息订阅回调函数
void subLeftImage_cb(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cout << " image_L receive success " << endl;
        cv::imshow("image_l",cv_bridge::toCvShare(msg,"bgr8")->image);
        cv::waitKey(10);//因为highgui处理来自cv::imshow()的绘制请求需要时间 10代表10ms
    }
    catch (cv_bridge::Exception& e)
    {
        cout << "Could not convert from " << msg->encoding.c_str() << "to 'brg8'." << endl;
    }   
}
void subRightImage_cb(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cout << " image_R receive success " << endl;
        cv::imshow("image_r",cv_bridge::toCvShare(msg,"bgr8")->image);
        cv::waitKey(10);//因为highgui处理来自cv::imshow()的绘制请求需要时间 10代表10ms
    }
    catch (cv_bridge::Exception& e)
    {
        cout << "Could not convert from " << msg->encoding.c_str() << "to 'brg8'." << endl;
    }   
}
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "subscribImage");

    ros::NodeHandle nh;//创建句柄
    image_transport::ImageTransport it(nh);

    image_transport::Subscriber sub_L;
    image_transport::Subscriber sub_R;

    //设置订阅主题 camera/image
    sub_L = it.subscribe("stereocamera/left/image_raw",1,subLeftImage_cb);
    sub_R = it.subscribe("stereocamera/right/image_raw",1,subRightImage_cb);

    ros::spin();

    return 0;
}

 5、测试

启动仿真

rlaunch robot_sim_demo robot_spawn.launch

运行 图像接受程序

rosrun opcv_ros subscribImage