はじめに
この記事は研究室の学生の教育用に作成してゆきます．
現在はROS2がメジャーとなってきておりますが，まずは自習が出来るよう文献が豊富なROS1でROSの扱いに慣れてみます．
ROSは既に多くの文献があり，それだけでも自習可能ですが解説されているバージョンや環境が違い，初心者には躓きやすい内容だと思ったため，改めてまとめてみます．
前回はこちら
Raspberry Pi 4 を使ったROS入門 第5回 RvizとSLAM体験
次回はこちら
公開までお待ちください
動作環境
Raspberry Piを使用します．(用意できるならUbuntu入りのPCが◎)
ROSはUbuntu上で動くソフトなのですが，設定の違いによってトラブルが起きやすいので，クリーンインストールしてから始めることをお勧めします．
ROSのバージョンはUbuntuのバージョンとリンクしてます．
本記事で使用する機器とバージョン
| 使用機器 | Ubuntuバージョン | ROSバージョン |
|---|---|---|
| Raspberry Pi 4 Model B / 8GB | 20.04 | Noetic Ninjemys |
※RAMが2GB，4GBだとビルドが途中で落ちることが多々ありました．
出来れば8GBが良いかもしれません．
1.ジョイスティックによるロボット操作
ロボットの動作テストや操縦にはジョイスティックがよく使用されます．
ROSではOS側が認識さえしていれば様々なコントローラーを使用することが出来ます．
今回はROSでも文献やライブラリが豊富なPS4コントローラ(DUALSHOCK4 Wireless Controller)を使ってみます．
下記コマンドでパッケージをインストールします．
sudo pip install ds4drv
sudo apt install ros-noetic-joy
実行してみます．PCのBluetoothがONになっている事を確認し，下記コマンドを実行してください．
sudo ds4drv
PSボタンとSHAREボタンを同時に長押しするとライトが白色に点滅します．この状態でしばらく待つと接続できます．接続するだけではボタンの入力情報は取得できません．joyを起動します．
roscore
rosrun joy joy_node
rostopicによりトピックの状態を確認してみます．
rostopic echo /joy
以下のように表示されます．
---
header:
seq: 37622
stamp:
secs: 1661154790
nsecs: 375230354
frame_id: "/dev/input/js0"
axes: [-0.0, -0.0, -0.0, 1.0, 1.0, -0.0, -0.0, -0.05376558378338814, -0.4619426131248474, -0.0, -0.0, -0.0, -0.0, -0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---
あとはこのtopicをSubscribeしてその値に応じて動作を変えるプログラムを作成すればロボットの操縦が出来ます．
2.C++によるノードの作成
ここまではPythonを使ってプログラムの作成，説明を行ってきました．Pythonは言語を覚えるのが簡単で，コンパイルも必要ありません．C＋＋はその裏返しにコンパイルが必要で言語を理解するには努力が必要です．しかし，C＋＋はPythonに比べて実行速度が速く，ロボットに必要なライブラリはC++であることが多いです．本格的なロボットのプログラムを作成する際にはC++は避けては通れません．今回は1章で説明したジョイスティックの指令を受け取って，速度を出力するプログラムを作ってみます．下記の通り,ファイルを作成してください．
//ROSの標準的な機能を使うためのヘッダ ros::init()やros::spinを使うために必要
#include <ros/ros.h>
//メッセージのヘッダです．Pythonのfrom sensor_msgs.msg import joyなどに相当
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
//C++のクラスを作る
class JoyTwist
{
//クラスの作成時に呼ばれるコンストラクタを定義
public:
JoyTwist()
{
//PublisherやSubscriberを作るときにC++ではros::NodeHandleのインスタンスを使う
ros::NodeHandle node;
//Subscriberを作成 joyという名前でキューのサイズが1，メッセージを受信したときの処理がjoyCallbackという関数になるように登録
joy_sub_ = node.subscribe("joy", 1, &JoyTwist::joyCallback, this);
//Publisherの作成 geometry_msgs::Twistという型をテンプレート引数として与えている
twist_pub_ = node.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
//コールバック関数の定義 const sensor_msgs::Joy&型として受け取る
void joyCallback(const sensor_msgs::Joy &joy_msg)
{
//メインのロジック ROSの配列メッセージはC++ではstd::vectorに変換される 本来はjoy_msg.axes.size()>=2などのサイズチェックが必要
if (joy_msg.buttons[0] == 1)
{
geometry_msgs::Twist twist;
twist.linear.x = joy_msg.axes[1] * 0.5;
twist.angular.z = joy_msg.axes[0] * 1.0;
twist_pub_.publish(twist);
}
}
//クラスのメンバとしてros::Subscriber,ros::Publisherを持たせている
private:
ros::Subscriber joy_sub_;
ros::Publisher twist_pub_;
};
//メイン関数 ros::initでROS Masterにノードとして登録 ノード名はros_twist
int main(int argc, char **argv) {
ros::init(argc, argv, "joy_twist");
JoyTwist joy_twist;
ros::spin();
}
コンパイルを行うためにCMakelistの2か所を下記の通り書き換えます．
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/beginner_tutorial_node.cpp)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(joy_twist src/joy_twist.cpp)
## Specify libraries to link a library or executable target against
#target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
## Specify libraries to link a library or executable target against
target_link_libraries(joy_twist
${catkin_LIBRARIES}
)
書き換えが終わったら保存とビルドを行います．この操作が問題なく行えると，catkin_ws/devel/lib/beginner_tutorialの中に，実行ファイル(joy_twist)が出来ています．プログラムを実行するために直接このファイルを実行も可能ですが，rosrunを使った方が簡単です．
試しにGazebo上のtortlebot3を動かしてみましょう．
roscore
rosrun joy joy_node
roslaunch turtlebot3_gazebo turtlebot3_world.launch
rosrun beginner_tutorial joy_twist
□ボタンを押しながら左スティックでロボットが動くはずです．
rqt_graphでノード間のやり取りを確認してみます．
rqt_graph
3.USBカメラの接続
ロボットのセンサーとしてよく使われるカメラを使ってみましょう．ROSではパッケージが用意されており，かなり簡単にカメラを接続することが出来ます．USBカメラを接続し，下記コマンドで接続を確認します．(ノートPCを使用していて内蔵のカメラを使っている場合も使用可能)
/dev/video0と表示されれば問題ありません．
ls /dev/video*
ROSに対応したUSBカメラを買う場合，UVC規格という規格の物を選択してください．
次に下記コマンドでドライバーをインストールします．
sudo apt-get install ros-noetic-usb-cam
試しに，下記コマンドでカメラで取得した画像をpublishしてみます．
roscore
rosrun usb_cam usb_cam_node
/usb_cam/camera_infoにカメラ情報がpublishされ/usb_cam/image_rawに画像がpublishされます．
ROSで画像を確認するにはimage_viewを使います．
rosrun image_view image_view image:=/usb_cam/image_raw
GUIが表示され，カメラで取得した画像が表示されていれば成功です．
4.OpenCVの活用
Open CV(Open Source Computer Vision Library)は画像・動画に関する処理機能をまとめたオープンソースのライブラリです．Intelが開発したライブラリで、OSSとして提供されているため、誰でも無料で使うことができます。趣味範囲の利用はもちろん、商業目的の利用も問題ありません．ロボットはカメラを使い，周囲の状況変化を読み取る場面がよくあります．Pythonは人工知能分野でも多く利用され，Open CVと組み合わせることにより，比較的簡単に様々な画像認識を行う事が出来るようになります．
cv_bridgeはOpenCVの標準データ形式であるcv::MatをROSのメッセージであるsensor_msgsに変換します．その逆も行えます．
以下にUSBカメラでキャプチャした画像をROSで転送し，OpenCVで画像処理した後に再びROSで転送するプログラムを示します．実際に変換を行っているのは17行目のcv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')の部分で返還後の形式にbgr8を選択しています．
#!/usr/bin/env python3
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class ColorExtract(object):
def __init__(self):
self._image_pub = rospy.Publisher('masked_image', Image, queue_size=1)
self._image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.callback)
self._bridge = CvBridge()
def callback(self, data):
try:
cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
except CvBridgeError as e:
print(e)
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
lower_blue = np.array([110,50,50])
upper_blue = np.array([130,255,255])
color_mask = cv2.inRange(hsv, lower_blue, upper_blue)
res = cv2.bitwise_and(cv_image, cv_image, mask=color_mask)
try:
self._image_pub.publish(self._bridge.cv2_to_imgmsg(res, 'bgr8'))
except CvBridgeError as e:
print(e)
if __name__ == '__main__':
rospy.init_node('color_extract')
color = ColorExtract()
try:
rospy.spin()
except KeyboardInterrupt:
pass
下記コマンドで実行してみます．
roscd beginner_tutorial/scripts/
chmod +x color.py
roscore
rosrun usb_cam usb_cam_node
rosrun beginner_tutorial color.py
rosrun image_view image_view image:=/masked_image
カメラの青い部分だけが抽出されている事が分かります．
この様にOpenCVは簡単にフィルターをかけることが出来ます．
これを利用して，ある一定面積青い色を検出したら前進し，
赤色の物が見えたら後退するプログラムを作成してみます．
#!/usr/bin/env python3
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
class ColorExtract(object):
def __init__(self):
self._vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self._blue_pub = rospy.Publisher('blue_image', Image, queue_size=1)
self._red_pub = rospy.Publisher('red_image', Image, queue_size=1)
self._image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.callback)
self._bridge = CvBridge()
self._vel = Twist()
def get_colored_area(self, cv_image, lower, upper):
hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
mask_image = cv2.inRange(hsv_image, lower, upper)
extracted_image = cv2.bitwise_and(cv_image, cv_image, mask=mask_image)
area = cv2.countNonZero(mask_image)
return (area, extracted_image)
def callback(self, data):
try:
cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
except CvBridgeError as e:
print(e)
blue_area, blue_image = self.get_colored_area(
cv_image, np.array([50,100,100]), np.array([150,255,255]))
red_area, red_image = self.get_colored_area(
cv_image, np.array([150,100,150]), np.array([180,255,255]))
try:
self._blue_pub.publish(self._bridge.cv2_to_imgmsg(blue_image, 'bgr8'))
self._red_pub.publish(self._bridge.cv2_to_imgmsg(red_image, 'bgr8'))
except CvBridgeError as e:
print(e)
rospy.loginfo('blue=%d, red=%d' % (blue_area, red_area))
if blue_area > 1000:
self._vel.linear.x = 0.5
self._vel_pub.publish(self._vel)
if red_area > 1000:
self._vel.linear.x = -0.5
self._vel_pub.publish(self._vel)
if __name__ == '__main__':
rospy.init_node('color_extract')
color = ColorExtract()
try:
rospy.spin()
except KeyboardInterrupt:
pass
実行してみます．
roscd beginner_tutorial/scripts/
chmod +x color_vel.py
roscore
roslaunch turtlebot3_gazebo turtlebot3_world.launch
rosrun usb_cam usb_cam_node
rosrun beginner_tutorial color_vel.py
デバッグの為に画像処理後の状態を表示しておきます．
rosrun image_view image_view image:=/blue_image
rosrun image_view image_view image:=/red_image
rosrun image_view image_view image:=/usb_cam/image_raw
青い画像と赤い画像を見つけた場合でそれぞれロボットが前進，後退すれば成功です．
OpenCVは他にも物体の形状を認識したり，人物の顔を認識したり，QRコードを読み取ったり，様々なライブラリが公開されています．それとROSを組み合わせることで様々な自律走行を可能にします．
一応下に使用した色見本を示しておきます．
印刷するか，スマホなどにこの画像を表示して,動かしてみてください．
おわりに
今回はジョイスティックとカメラを使ってロボットを操縦してみました．
自分で0から実装しようと思うとかなり大変ですが，
ROSを使うとかなり簡単に実装できたと思います．
よいROSライフを．