はじめに
ROS2のプログラミングの勉強（基礎編）とし実施したことをまとめて残していきます．
このページでは下記2つのプログラムを合わせて１つにしたい場合の例を示します．
ROS2プログラミング基礎_1-3: ゲームパッド操作プログラムの自作
https://qiita.com/pez/items/9d100183d8bac3b9e18f
ROS2プログラミング基礎_3-2: TurtleBot3のUSBカメラ画像から特徴検出
https://qiita.com/pez/items/99ed918900f7c8517c13
環境
・PC: Panasonic Let's Note CF-FV
・OS: Windows11 Pro (64bit)
・VirtualBox: 6.1.38 (Ubuntu20.04.5, GUI環境)
・ROS: ROS2 Foxy
・ロボット：
Turtlebot3
burger (Raspberry Pi 3B+/4B) or Waffle (Raspberry Pi 3B+/4B)
USBカメラ（例えばLogicool C270n HD WEBCAM など）が接続された状態
・ゲームパッド：Logicool F710
ここまでの環境構築については下記参照
・Windows PCにVirtualBox＋Ubuntuを導入
https://qiita.com/pez/items/a3ef1855f7e1e0ed3dfd
・ROS(ROS2 Foxy)をインストール
https://qiita.com/pez/items/1df36628524ff40a3d93
・Turtlebot3 burger のセットアップ
https://qiita.com/pez/items/1d3d15b3911d5dab1702
1. ゲームパッド接続・設定
↓こちらは実施済みとします
ROS2プログラミング基礎_1-3: ゲームパッド操作プログラムの自作
https://qiita.com/pez/items/9d100183d8bac3b9e18f
2. ワークスペース作成
今回も前回使った teleop_ws を再利用することにします．もちろん新しいワークスペースを作成してもOKです．
$ cd ~/teleop_ws/src/
3. パッケージ作成
今回ノード名は my_imcon_node
パッケージ名は my_imcon
にすることにします
$ cd ~/teleop_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_imcon_node my_imcon
4. プログラミング
4.1 package.xml ファイルの編集
$ cd ~/teleop_ws/src/my_imcon
$ emacs package.xml
自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．
4.2 setup.py ファイルの編集
$ cd ~/teleop_ws/src/my_imcon
$ emacs setup.py
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．
entry_point={
'console_scripts': [
'my_imcon_node = my_imcon.my_imcon_node:main'
],
},
※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．
4.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_imcon_node.py を編集することにします．
$ cd ~/teleop_ws/src/my_imcon/my_imcon
$ emacs my_imcon_node.py
これまでのプログラム
(1-3)my_gamepad_node.py と (3-2)my_image_canny_node.py を
単純に合体させたものがこちらになります．
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge
import sys
from geometry_msgs.msg import Twist # Twist メッセージ型をインポート
import pygame
from pygame.locals import *
class MyImcon(Node):
def __init__(self):
super().__init__('my_imcon')
self.subscription = self.create_subscription(
Image,
'/image_raw',
self.image_callback,
qos_profile_sensor_data)
self.publisher = self.create_publisher(
Image,
'edges_result', 10)
self.publisher2 = self.create_publisher(Twist, 'cmd_vel', 10)
self.timer = self.create_timer(0.01, self.timer_callback)
self.vel = Twist() # Twist メッセージ型インスタンスの生成
self.vel.linear.x = 0.0
self.vel.angular.z = 0.0
self.br = CvBridge()
# ジョイスティックの初期化
pygame.joystick.init()
try:
# ジョイスティックインスタンスの生成
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
print('ジョイスティックの名前:', self.joystick.get_name())
print('ボタン数 :', self.joystick.get_numbuttons())
except pygame.error:
print('ジョイスティックが接続されていません')
# pygameの初期化
pygame.init()
def image_callback(self, data):
frame = self.br.imgmsg_to_cv2(data, 'bgr8')
frame_grayscale = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(frame_grayscale, threshold1=100, threshold2=200)
edges_result = self.br.cv2_to_imgmsg(edges, 'passthrough')
self.publisher.publish(edges_result)
cv2.imshow('Original Image', frame)
cv2.imshow('Canny Edge Detection', edges)
cv2.waitKey(1)
def timer_callback(self): # タイマーのコールバック関数
for e in pygame.event.get():
linearspeed = 0.0
rotation = 0.0
if e.type == QUIT:
break
if e.type == pygame.locals.JOYAXISMOTION:
linearspeed = - float(self.joystick.get_axis(1)) * 0.2
rotation = - float(self.joystick.get_axis(0)) * 0.8
self.vel.linear.x = linearspeed
self.vel.angular.z = rotation
self.publisher2.publish(self.vel) # 速度指令メッセージのパブリッシュ
self.get_logger().info(f'並進速度={self.vel.linear.x} 角速度={self.vel.angular.z}')
def main():
rclpy.init()
node = MyImcon()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
・最初のモジュールを読み込む部分は重複があれば消すぐらいでOK
・classの名前は新しくMyImconとしました
・__init(self)' は最初に１回だけ実行されるものでありそれぞれのプログラムのこの部分を統合しています．ただし「publisher」の名前がかぶる部分があるため一方を「publisher2」と変更しています
・残りの「image_callback」と「timer_callback」はそのまま並べてコピーしてきています．速度指令のパブリッシュのところだけ「publisher2」に修正しました．
・main関数はクラス名MyIconにあわせて修正しました．
5. ビルド
ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます
$ cd ~/teleop_ws
$ colcon build
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます
6. 設定ファイルの反映
アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既にある
source /opt/ros/foxy/setup.bash
オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
$ emacs ~/.bashrc
$ source ~/teleop_ws/install/setup.bash
を追加してから反映させる
$ source ~/.bashrc
7. ノードの実行（実機）
今回はロボットを動かすプログラムなのでロボットを起動します．
TurtleBot3の設定については別記事参照．
https://qiita.com/pez/items/1d3d15b3911d5dab1702
$ ros2 launch turtlebot3_bringup robot.launch.py
$ ros2 run usb_cam usb_cam_node_exe
作成したノードの実行
$ ros2 run my_imcon my_imcon_node
画像処理を行った画像が表示されると同時にゲームパッドでの操作が可能になるはずです．