目的
Turtlebotに搭載されているKinectは，カラー画像と距離が取得できるセンサーです．本演習では，ROSを介してKinectより得られる画像を用いたプログラミング方法について学びます．
Kinectからの画像取得と表示
VM環境下でubuntuを動かしている人はKinectに接続できません．チーム単位で本日の演習に取り組んでください．2017年まではopenniパッケージを使用していましたが，エラーが多発するためfreenectパッケージを使用することにします．openniパッケージを使用したい方はこちらのページを参照してください．
まずは，既存のパッケージから 画像を取得，表示しましょう．kinectに接続し，画像のトピックを獲得するために下記のコマンドを実行します．
$ roslaunch freenect_launch freenect.launch
次に画像を表示する2つの方法について説明します．1つ目は，rvizを用いた画像を表示する方法です．下記のコマンドを実行してrvizを立ち上げましょう．
$ rosrun rviz rviz
2つ目はimage_viewを使用する方法です．端末で下記を実行しましょう．
$ rosrun rqt_image_view rqt_image_view
ウィンドウが立ち上がるので，GUIから表示したい画像トピックを選択してください．カラー画像は/camera/rgb/image_color，距離画像は/camera/depth/imageとなります．
Kinectによる画像処理
1. パッケージの作成
kinectを扱う場合には，依存パッケージとして下記を指定してください．なお，本ページではパッケージの名前をdepth_estimaterとして話を進めます．
cv_bridge roscpp sensor_msgs std_msgs
2. プログラミング
それでは，kinectから得られたRGB画像とD画像を処理するプログラムを書きます．今回は，RGB画像の注目する領域(ROI領域)を赤く表示し，そのROI領域の平均距離を計算するプログラムを作成します．
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#define WIDTH 50
#define HEIGHT 25
class depth_estimater{
public:
depth_estimater();
~depth_estimater();
void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
private:
ros::NodeHandle nh;
ros::Subscriber sub_rgb, sub_depth;
};
depth_estimater::depth_estimater(){
sub_rgb = nh.subscribe<sensor_msgs::Image>("/camera/rgb/image_color", 1, &depth_estimater::rgbImageCallback, this);
sub_depth = nh.subscribe<sensor_msgs::Image>("/camera/depth/image", 1, &depth_estimater::depthImageCallback, this);
}
depth_estimater::~depth_estimater(){
}
void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
}
void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/5.5));
}else{
}
if(i > y1 && i < y2){
if(j > x1 && j < x2){
if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
cv::imshow("DEPTH image", img);
cv::waitKey(10);
}
int main(int argc, char **argv){
ros::init(argc, argv, "depth_estimater");
depth_estimater depth_estimater;
ros::spin();
return 0;
}
3. ビルド
画像処理ライブラリであるopencvを使用しているため，CMakeLists.txtにその旨を記述する必要があります．2箇所のハイライト部分を追加しましょう．
find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp sensor_msgs std_msgs ) find_package(OpenCV REQUIRED)
add_executable(depth_estimater src/depth_estimater.cpp)
target_link_libraries(depth_estimater ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
4. 実行
作成したプログラムを実行します．kinectから画像を取得するためには，先ほどのlaunchファイルをroslaunchします．
roslaunch openni_launch openni.launch
次に，rosrunにより作成したノードを起動しましょう．下記に作成したノードを実行した例を示します．
5. プログラムの内容
それでは，プログラムの内容を確認しましょう．
#include <ros/ros.h> #include <sensor_msgs/Image.h> #include <cv_bridge/cv_bridge.h> #include <opencv2/opencv.hpp>
必要なヘッダファイルを読み込んでいます．Open CVを使用する場合には，多くのヘッダファイルが必要になるので，全て記述しましょう．
#define WIDTH 50 #define HEIGHT 25
赤枠の大きさです．実際には，(WIDTH*2)x(HEIGHT*2)の画素数となるので注意してください．
class depth_estimater{
public:
depth_estimater();
~depth_estimater();
void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
private:
ros::NodeHandle nh;
ros::Subscriber sub_rgb, sub_depth;
};
depth_estimaterというクラスを定義し，幾つかのクラスメンバを宣言しています．カラー画像を購読した際にはrgbImageCallback関数，距離画像を購読した際にはdepthImageCallback関数が実行されます．
void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
}
カラー画像を購読した際には上記の関数が実行されます．詳細を見ましょう．
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
変数を宣言しています．cv_ptrは画像を扱うためのものです．
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
画像を購読し，BGR8形式(1チャンネル(青，緑，赤のどれか1つ)につき8ビット(0~255階調))のカラー画像に変換し，cv_ptrに代入しています．ここで注意すべき点は，チャンネルの順番がBGR，すなわち青，緑，赤の順になっている点です．また，画像を購読できない場合には，エラーを出力します．そして，cv_ptrをmatに代入しています．
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
赤枠の大きさを設定しています．
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
赤枠内の画素にアクセスし，青と緑のチャンネルに0を代入しています．チャンネルの順番は，青，緑，赤です．画素にアクセスしているプログラムは3行ありますが，先ほどのチャンネルの順番となっているので注意しましょう．画素にアクセスする方法は幾つかあります．アクセス方法によっては，時間がかかる場合があるので注意が必要です．アクセスの仕方についてはこちらのページを参考にしましょう．
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
最後に，rectangle関数で枠の外側を赤色で囲っています．そして，imshow関数により画像を表示しています．waitKey関数はキーが押されるまで待機する関数です．今回はキー入力を必要としませんが，画像を表示する場合には必要な関数なので記述しましょう．
void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/5.5));
}else{
}
if(i > y1 && i < y2){
if(j > x1 && j < x2){
if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
cv::imshow("DEPTH image", img);
cv::waitKey(10);
}
距離画像を購読した際には上記の関数が実行されます．もう少し詳細を見ましょう．
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
カラー画像と同様に変数を宣言しています．
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
上記もカラー画像とほぼ同じ内容です．異なる点は，TYPE_32FC1形式に変換している点です．TYPE_32FC1は，1チャンネル浮動小数点数(1つの画素に32ビット)を表しています．
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
指定した大きさの配列を確保しています．CV_8UC1は，先ほど出てきたBGR8と同じです．
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
枠の大きさを設定しています．
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/5.5));
}else{
}
if(i > y1 && i < y2){
if(j > x1 && j < x2){
if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
枠内の距離値の合計を計算しています．その際，もし距離が0[m]以下であれば，除外しております．先ほどとは画像のアクセスの仕方が異なります．
CV_32FC1型では，dataメソッドを使用して画素にアクセスすることができないため，ptrメソッドを使用しています．また，距離画像を可視化するために，100行目では距離値を取得できる最大距離で割り，255を掛けています．255は8ビットで表現できる最大の数です．最後に，114行目で枠内の画素数で割り，計算した平均距離を次の行で出力しています．
cv::imshow("DEPTH image", img);
cv::waitKey(10);
カラー画像を購読する関数と同様に，可視化した距離画像を表示しています．
int main(int argc, char **argv){
ros::init(argc, argv, "depth_estimater");
depth_estimater depth_estimater;
ros::spin();
return 0;
}
以前と変わらない処理なので説明を省略します．
課題
Turtlebotが前進している際に，Turtlebotと前方の壁までの距離が1.0m以下になったら停止しなさい．