1. 程式人生 > >gazebo中建立stereo camera並且利用ROS獲取左右圖

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;
}