久々にROS2を触りました。ROS2使いを名乗っておいてその姿勢はダメだろ！とは思いますが、ROS1が研究でのスタンダードなもので…。
Realsenseとは
おそらく、皆さんご存知だと思いますが、RealsenseはIntel社が開発・販売を行っている高性能デプスカメラシリーズとして注目されている製品です。Kinect(旧)の後継として注目されていた気がします。
Realsenseは価格が3万円前後と(工学用途としては)非常に安価な上にSDKやAPIが充実しています。ROSに対応しており、ROS2へのサポートも手厚いようです。
私は、RealsenseはROS1でのみ使用していたため、今回は、RealsenseのROS2での動作確認とともに、画像トピックの送受信確認を行いました。
環境構築
ROS2 Dashing(Ubuntu 18.04 LTS)を前提としています。
Githubに上がっているROS2_intel_realsenseのReadme通りに環境構築をすれば十分なので、説明は省きます。特にトラブらず、インストールが完了しました。
プログラミング
ソースコードはPlaygroundを作るレベルに多くならない限りUPしないと思います。
ROSで面倒なデータ型はいろいろありますが、画像の受信・送信は少し手こずります。メッセージの送受信のためには、Image_transport、sensor_msgs/image、OpenCVの3種類のが必要です。CMakeLists.txtを示します。
cmake_minimum_required(VERSION 3.5)
project(realsense_subscriber)
set(CMAKE_CXX_STANDARD 14)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(image_transport REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
set(OpenCV_INCLUDE_DIRS)
set(OpenCV_LIB_DIR)
#------------------------------------------------
include_directories(realsense_sub
${OpenCV_INCLUDE_DIRS}
)
add_executable(realsense_sub
src/realsense_sub.cpp
)
ament_target_dependencies(realsense_sub
rclcpp
image_transport
cv_bridge
std_msgs
sensor_msgs
# ament_cmake
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
install(TARGETS
realsense_sub
DESTINATION lib/${PROJECT_NAME}
)
#------------------------------------------------
ament_package()
プログラム。特に詳細は言いませんが、簡単に説明すると、画像メッセージを受信したらカスケード分類器で物体位置推定をしてimshowをするだけのプログラムです。xmlファイルのパスは絶対パスのほうがいいような気がします。あと、カスケードファイルはOpenCV公式から落としてくることをおすすめします。
#include <rclcpp/rclcpp.hpp>
//#include <sensor_msgs/image_encodings.hpp>
#include <image_transport/image_transport.h>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
using std::placeholders::_1;
int initial_flag = 0;
std::string window_name = "image";
class realsens_subscriber:public rclcpp::Node
{
public:
realsens_subscriber():Node("realsense_subscriber")
{
cascade.load("./haarcascade_fullbody.xml");
sub_ = this->create_subscription<sensor_msgs::msg::Image>("camera/color/image_raw", 1, std::bind(&realsens_subscriber::realsense_callback, this, _1));
cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);
}
~realsens_subscriber()
{
cv::destroyAllWindows();
}
private:
cv::CascadeClassifier cascade;
std::vector<cv::Rect> contour;
void realsense_callback(const sensor_msgs::msg::Image::SharedPtr msg);
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
};
void realsens_subscriber::realsense_callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
auto data = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
cv::Mat gray;
cv::cvtColor(data->image, gray, cv::COLOR_BGR2GRAY);
cascade.detectMultiScale(data->image, contour, 1.1, 3, 0, cv::Size(30, 30));
for(int i = 0; i < contour.size(); i++){
cv::rectangle(data->image, cv::Point(contour[i].x, contour[i].y), cv::Point(contour[i].x + contour[i].width, contour[i].y + contour[i].height), cv::Scalar(255, 0, 0), 1);
}
cv::imshow(window_name.c_str(),data->image);
cv::waitKey(1);
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<realsens_subscriber>());
rclcpp::shutdown();
return 0;
}
あとは、Colconでビルドして実行するだけ。Realsenseのノードが立ち上がっていることを忘れずに。
点群と組み合わせれば、距離測定とかもできると思います。
以前は、Image_transportを使えず使用を諦めていたので動く前例を作れたのは大きいと思っています。卒研発表まで2週間近いので、毎日投稿は（ネタ的にも時間的にも）かなり厳しいですが、ちまちまと日記として綴ることはしたいと思っています。
