現在很多CNN演算法都開始採用無監督的學習演算法，有的時候需要在模擬環境中獲取一個stereo camera的左右圖來進行訓練，發現用gazebo獲取一個stereo camera的左右圖的資料比較少，所以就記錄一下我做的過程。在讀這篇部落格之前，請先了解gazebo的相關內容和ROS的相關知識。

步驟：
1、建立一個stereo camera model，新增ros plugin
2、建立一個gazebo world file
3、建立一個ROS workspace 來儲存stereo camera的左右圖

我的系統是Ubuntu 14.04，gazebo的版本是預設的gazebo 2.2

1、建立stereo camera model
  gazebo支援兩種模型的描述格式，URDF和SDF檔案，SDF檔案出的比較晚，書寫起來比較方便，建議使用SDF檔案，但是原來有很多model的描述都是URDF的檔案格式的，gazebo也是可以同時相容的，URDF檔案的字尾是.xacro。
  下面是我的stereo camera的sdf檔案

    <model name="kinect_ros">
      <pose>-2 2.5 1 0 0 0</pose>
      <link name="link">
        <inertial
 
>
          <mass>0.1</mass>
        </inertial>

        <collision name="collision">
          <geometry>
            <box>
              <size>0.073000 0.276000 0.072000</size>
            </box>
          </geometry>
        </collision>
        <visual
 
 name="visual">
          <geometry>
            <mesh>
              <uri>model://kinect_ros/meshes/kinect_ros.dae</uri>
            </mesh>
          </geometry>
        </visual>
            <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
          <image>
            <format>B8G8R8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.4</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_depth_optical_frame</frameName>
          <baseline>0.1</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>
          <pointCloudCutoff>0.4</pointCloudCutoff>    <--! max distance -->
        </plugin>
      </sensor>
        <gravity>0</gravity>
      </link>
    </model>

上面就是stereo camera的model的sdf檔案描述，整個部分被 “model”包括，這也是和其它model的區分的界限。“camera” 片段之間描述的就是相機的內部引數，可以修改圖片大小（640 x 480）,顏色通道（B8G8R8），可測量的距離（near和far），然後新增ROS plugin就可以釋出一些關於圖片的rostopic，用這些topic來讀取gazebo中的相機的左右圖。上述檔案中還有個mesh引數，匯入的是相機的外形引數，我直接用的是kinect的mesh。

2、建立一個gazebo world file
  world file是來描述這個虛擬世界的一些基本的元素，比如說 “ground plane”，”sun”等，還有除了你所新增的機器人以外的環境（比如桌子、椅子、房子等）。這一個步驟推薦利用圖形介面來完成，要是都是通過直接寫sdf檔案來描述得累死，也沒有必要。圖形介面直接插入gazebo model base裡面的model，然後直接儲存成為一個sdf檔案，開啟之後就可以看到了詳細的sdf檔案描述。
  接下來有兩種辦法將world file和上面的stereo camera的model file合併起來：
1、通過一個launch file，分別新增world file和model file
2、直接將model file插入到world file中。

我採用的是第二種，這樣就一個檔案，直接rosrun gazebo_ros gazebo xxx.world就可以查看了

3、建立一個workspace來讀取雙目相機的左右圖
直接上程式碼了：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include "std_msgs/Empty.h"

using namespace std;
using namespace cv;


static const std::string OPENCV_WINDOW = "Image window";
static const std::string OPENCV_WINDOW2 = "Image window2";


//void imageCb(const sensor_msgs::ImageConstPtr& msg);

void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
int count = 0;

    char t[100];

    sprintf(t,"%08d",count);    
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);  //bgr8
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat frame = cv_ptr->image;
    cv::imshow(OPENCV_WINDOW, frame);
    cv::imwrite(std::string("/home/osgoodwu/save_stereo_workspace/left")+t+std::string(".png"),frame);
   // count++;
    cv::waitKey(3);
}

void imageCb2(const sensor_msgs::ImageConstPtr& msg)
{
int count = 0;

    char t[100];

    sprintf(t,"%08d",count);    
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);  //bgr8
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat frame = cv_ptr->image;
    cv::imshow(OPENCV_WINDOW, frame);
    cv::imwrite(std::string("/home/osgoodwu/save_stereo_workspace/right")+t+std::string(".png"),frame);
   // count++;
    cv::waitKey(3);
}

int main(int argc, char** argv)
{
    //static int count = 0;
    ros::init(argc, argv, "image_converter");

    ros::NodeHandle nh_;
    image_transport::ImageTransport it_(nh_);
    image_transport::Subscriber image_sub_;
    image_transport::Subscriber image_sub2_;

    image_sub_ = it_.subscribe("/stereo/camera/left/image_raw", 1, imageCb);
    image_sub2_ = it_.subscribe("/stereo/camera/right/image_raw", 1, imageCb2);


    ros::Rate rate(10.0);

    // ImageConverter ic;1
    while(ros::ok())
    {

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}