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