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

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

1、仿真环境

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

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

2、双目相机 sdf文件

  <!-- stereo camera -->
    <gazebo reference="camera_link">  
      <sensor type="multicamera" name="stereocamera">
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <visualize>false</visualize>
        <camera name="left">
          <pose>0 0 0 0 0 0</pose>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>
        <camera name="right">
          <pose>0 -0.07 0 0 0 0</pose>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>        
        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <cameraName>stereocamera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <cameraName>stereocamera</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link_optical</frameName>
          <baseline>0.07</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
        </plugin>
      </sensor>
    </gazebo>

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

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

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

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

#include <ros/ros.h>  
#include<sensor_msgs/image_encodings.h> //ROS图象类型的编码函数
#include<image_transport/image_transport.h> //用来在ROS系统中的话题上发布和订阅图象消息

//OpenCV2标准头文件
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<cv_bridge/cv_bridge.h>     //cv_bridge中包含CvBridge库

#include<iostream> //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