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