拙著（以下，教科書と表記）「ROS2とPythonで作って学ぶAIロボット入門」（講談社）を授業で使用したときの補足資料です．
６章マニピュレーションの補足サンプル．教科書では，P192のsimple_armをGUIで動かしていましたが，それをプログラムで動かすサンプルです．
参考サイト
コード
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from sensor_msgs.msg import JointState
import time
import threading
# Simple arm用のクラス
class Commander(Node):
def __init__(self):
super().__init__('commander')
# パブリッシャの生成
self.publisher_joint = self.create_publisher(JointState, 'joint_states', 10)
def publish_joint(self, joint_name, q):
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = [joint_name] # nameはリストでないとだめなので[]を付けている
msg.position = [q] # positionもリスト
self.publisher_joint.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg)
def main(args=None):
rclpy.init(args=args) # 初期化
commander = Commander() # ノードの作成
# 別のスレッドでrclpy.spin()を呼び出す
thread = threading.Thread(
target=rclpy.spin, args=(commander, ), daemon=True)
thread.start()
time.sleep(1.0) # 1秒待つ
# 目標角度の設定
target_joint1 = 0.0
target_joint2 = 0.0
commander.publish_joint('joint1',target_joint1)
commander.publish_joint('joint2',target_joint2)
print('Move: Input 1, 2, 3, 4 key')
print('Space: Initial position')
print('Esc: Quit')
try:
while rclpy.ok():
c = input()
# 押されたキーによって場合分けして処理
if c == '1':
target_joint1 -= 0.1
elif c == '2':
target_joint1 += 0.1
elif c == '3':
target_joint2 -= 0.1
elif c == '4':
target_joint2 += 0.1
elif c == ' ': # Space key
target_joint1 = 0.0
target_joint2 = 0.0
elif ord(c) == 27: # Escape key
break
commander.publish_joint('joint1',target_joint1)
commander.publish_joint('joint2',target_joint2)
print("joint1",target_joint1,"joint2",target_joint2)
time.sleep(0.01)
except KeyboardInterrupt: # Ctrol-cでエラーにならないように
pass
rclpy.shutdown()
thread.join() # スレッドが終了するまで待つ
print('End')
ハンズオン
- AIロボット入門のDockerイメージを使うことが前提
- パッケージの作成
- パッケージの作り方を思い出して（教科書第２章），simple_armパッケージとsimple_armノードを作成する．
- コードの作成
- 上のソースコードをコピペして以下のファイルを作成する．
~/happy_ws/src/simple_arm/simple_arm/simple_arm.py
- 上のソースコードをコピペして以下のファイルを作成する．
- ビルド
$ cd ~/happy_ws$ colcon build
- 実行
- RVizの起動
- 次のコマンドを実行するとRViz（上図右）とJoint State Publisher（上図左）が起動する．
$ ros2 launch simple_arm_description display.launch.py
- 次のコマンドを実行するとRViz（上図右）とJoint State Publisher（上図左）が起動する．
- simple_armノードの起動
$ source ~/happy_ws/install/setup.bash$ ros2 run simple_arm simple_arm
- RVizの起動
- 遊び方
-
- Joint State Publisherウインドウ右上の×印を押して，Joint State Publisherを終了させる．これをしないとsimple_armノードが動かない．
- simple_armノードを起動した端末で次のキーを入力してEnterキーを押すとロボットが動く．
- １：関節角１を0.2[rad]を減少させる
- ２：関節角１を0.2[rad]を増加させる
- ３：関節角２を0.2[rad]を減少させる
- ４：関節角２を0.2[rad]を増加させる
- Spaceキー：初期姿勢に戻る
-
ホームワーク
- このサンプルコードは，キーを入力してからEnterキーを押さないとロボットアームが動ない．これをキーを入力すると直ちにロボットアームが動くように改良しよう！ヒントは以下のプログラム．
- 6.5.5 関節を動かすプログラム commander1.py
- 運動学，逆運動学を計算するメソッドをそれぞれ作り，それが正しいことを示すプログラムを作ろう！
終わり
コメント