はじめに
前回は、LIDAR のトピックの取得方法と、その処理方法について説明しました。
今回は、Gazebo 上でシミュレートしているカメラの映像を取得して障害物を検知する方法を説明します。
前提条件
前提条件は、以下の通りです。
- /image_raw トピックが存在している
- /cmd_vel トピックが存在している
上記トピックがあれば、問題ありません。
Gazebo上のカメラの映像を取得する
早速、カメラの映像を取得する方法を説明していきます。
今回使用するカメラのトピック名は、/camera/image_raw となります。
前回までの記事を完了した方は以下のコマンドで Python ファイルを作成してください。
cd ~/ros_ws/src/ros2_first_test/ros2_first_test
touch get_camera.py
映像を取得する get_camera.py は、以下の通りです。
import rclpy
import cv2
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
class getVideoFrame(Node):
def __init__(self):
# wake up node name
super().__init__("video_subscriber")
# declare subscriber
self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)
# setting cv2
self.bridge = CvBridge()
def get_camera_data(self, data):
# separate scan data
frame = self.bridge.imgmsg_to_cv2(data)
# show frame
cv2.imshow("Gazebo-camera1", frame)
cv2.waitKey(1)
def main(args=None):
rclpy.init(args=args)
gVF = getVideoFrame()
rclpy.spin(gVF)
rclpy.shutdown()
if __name__ == "__main__":
main()
コードの内容を説明します。ここでは、新たに登場した部分のみを扱います。
self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)
sensor_msgs/msg/Image は、公式サイトに記載のある通り、ヘッダーや画像のBGR値で構成されています。create_subscription によって、これを取得します。
次は、ROS2 の映像と OpenCV をつなぐために必須の CVBridgeクラスを用意します。
self.bridge = CvBridge()
self.get_camera_data では、この CVBridge を用いて画像を取得します。
# separate scan data
frame = self.bridge.imgmsg_to_cv2(data)
# show frame
cv2.imshow("Gazebo-camera1", frame)
cv2.waitKey(1)
CVBridge.imgmsg_to_cv2(camera_topic) により、sensor_msgs/msg/Image 型をもつカメラトピックから、画像の部分のみを抽出し、OpenCV で扱えるように、numpy に変換します。
cv2.imshow(“frame-name”, frame) は、frame-name という Window 名で frame の画像を表示するウィジェットを立ち上げます。
cv2.waitKey(1) は、約0.001s 待機します。cv2.waitKey(1000) にすると、約1s 待機します。
cv2.waitKey(0) は、キー入力があるまで待機します。
setup.pyの編集
ここまで準備できたら、setup.py に先ほど作成した get_camera.py を登録します。
'console_scripts': [
'translate_cmdvel=ros2_first_test.translate_cmdvel:main',
'get_imu=ros2_first_test.get_imu:main',
'get_lidar=ros2_first_test.get_lidar:main',
'get_camera=ros2_first_test.get_camera:main',
],
あとは、ビルドして実行するだけです。
カメラトピックをパブリッシュするための Gazebo シミュレーション環境を起動してください。
cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py
Gazebo が立ち上がったら、Gazebo上に適当な障害物を設置してください。
別のターミナルを開いて、以下を実行します。
ros2 run ros2_first_test get_camera
以下のような画面になれば、成功です。
![](https://nutritionfoodtech.com/wp-content/uploads/2022/10/camera1-1024x525.png)
障害物の検知
次は、OpenCVを用いて画像から障害物を検知できるようにします。
def get_camera_data 内を変更していきます。
def get_camera_data(self, data):
# separate scan data
frame = self.bridge.imgmsg_to_cv2(data)
image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
image = image[:600, :]
image = cv2.inRange(image, 0, 100)
# 輪郭抽出
contours, _ = cv2.findContours(image,
cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)
if len(contours) > 0:
contour = max(contours, key=lambda x: cv2.contourArea(x))
else:
contour = contours
frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)
# show frame
cv2.imshow("Gazebo-camera1", frame)
cv2.waitKey(1)
コードの解説をしていきます。
image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
image = image[:600, :]
まずは、処理を簡単にするために cv2.cvtColor を用いて画像をグレースケールに変更します。
続いて、そのままの画像では少しだけ自分の車体が映ってしまうので、image = image[:600, :] で、該当部分を削除します。
image = cv2.inRange(image, 0, 100)
inRange 関数でグレースケール画像の値が 0 ~ 100 の部分のみを抽出します。
contours, _ = cv2.findContours(image,
cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)
cv2.findContours で、抽出した画像から閉じた輪郭を検索します。
輪郭の数だけ、contours に保存されます。
if len(contours) > 0:
contour = max(contours, key=lambda x: cv2.contourArea(x))
else:
contour = contours
contours が 0 以上の長さを持つリストの場合、max 関数で最も輪郭の大きい値を抽出します。
frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)
最も大きい輪郭を、drawContours 関数で、元の画像に描画します。
ここまでできたら、ビルドして実行してみましょう。
cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py
別のターミナルを開きます。
ros2 run ros2_first_test get_camera
以下のような映像が表示されれば、完成です。
![](https://nutritionfoodtech.com/wp-content/uploads/2022/10/camera2-1024x623.png)
障害物の回避
ここまできたら、あとは /cmd_vel に、回避指令を出すだけです。
ルールとしては単純に、画像内に輪郭のある物体がなくなるまで回転→移動ということにします。
get_camera.py のコードは以下のようになります。
import rclpy
import cv2
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
class getVideoFrame(Node):
def __init__(self):
# wake up node name
super().__init__("video_subscriber")
# declare subscriber
self.subscription = self.create_subscription(Image, "/camera1/image_raw", self.get_camera_data, 10)
self.publisher = self.create_publisher(Twist, "/first_robot/cmd_vel", 10)
# period publisher
timer_period = 0.5
self.timer = self.create_timer(timer_period, self.send_cmd_vel)
# setting cv2
self.bridge = CvBridge()
self.velocity = Twist()
def get_camera_data(self, data):
# separate scan data
frame = self.bridge.imgmsg_to_cv2(data)
image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
image = image[:600, :]
image = cv2.inRange(image, 0, 100)
# 輪郭抽出
contours, _ = cv2.findContours(image,
cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)
if len(contours) > 0:
contour = max(contours, key=lambda x: cv2.contourArea(x))
self.x, self.y, _, _ = cv2.boundingRect(contour)
else:
contour = contours
self.x, self.y = -1, -1
frame = cv2.drawContours(frame, contour, -1, (0, 255, 0), 5)
# show frame
cv2.imshow("Gazebo-camera1", frame)
cv2.waitKey(1)
def send_cmd_vel(self):
if 400 >= self.x >= 0:
self.velocity.angular.z = -1.0
elif 800 >= self.x >= 400:
self.velocity.angular.z = 1.0
else:
self.velocity.angular.z = 0.0
self.publisher.publish(self.velocity)
def main(args=None):
rclpy.init(args=args)
gVF = getVideoFrame()
rclpy.spin(gVF)
rclpy.shutdown()
if __name__ == "__main__":
main()
def send_cmd_vel について説明します。
def send_cmd_vel(self):
if 400 >= self.x >= 0:
self.velocity.angular.z = -1.0
elif 800 >= self.x >= 400:
self.velocity.angular.z = 1.0
else:
self.velocity.angular.z = 0.0
self.x は、画像の障害物の中心x座標です。
障害物の中心が、画面半分より左側にある場合は、self.velocity.angular.z = -1.0 とします。
画像から障害物が消えた場合は、self.velocity.angular.z = 0.0 として、ロボットの回転を停止します。
早速、ビルドして実行していきます。
cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py
Gazebo を立ち上げた後に、画像処理プログラムを起動します。
ros2 run ros2_first_test get_camera
以下のようにロボットが動けば完了です。
![](https://nutritionfoodtech.com/wp-content/uploads/2022/10/camera-gif1.gif)
おわりに
簡単な画像処理と /cmd_vel を組み合わせて、障害物回避のプログラムを作成しました。
LIDAR や IMU、カメラのデータを上手に処理することができれば、自分なりのロボットモーションを作成することが可能になります。
次回からは、SLAM (自己位置推定) のプログラムを作成していきます。
URDF と launch ファイルの改良もしていきます。
コメント