gazebo中创建stereo camera并且利用ROS获取左右图

  现在很多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;
}

参考文献：
[1] http://gazebosim.org/tutorials