本記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今週はrvizを使い２自由度のロボットアームを作り、関節を動かします。2自由度ロボットアームのURDFは大阪電気通信大学の升谷先生が作られたものを使っています．このサンプルコードは升谷先生が作られた，参考サイトのcrane_plus_commander/commander1.pyを参考に，JointTrajectoryの代わりにJointStateを使ってロボットを動かしています．
参考サイト
コード
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.joint1_name = ['joint1']
self.joint2_name = ['joint2']
# パブリッシャの生成
self.publisher_joint1 = self.create_publisher(JointState, 'joint_states', 10)
self.publisher_joint2 = self.create_publisher(JointState, 'joint_states', 10)
def publish_joint1(self, q):
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = self.joint1_name # nameはリストでないとだめなので[]を付けている
msg.position = [q] # positionもリストの必要あり
self.publisher_joint1.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg)
def publish_joint2(self, q):
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = self.joint2_name
msg.position = [q]
self.publisher_joint1.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_joint1(target_joint1)
commander.publish_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 == ' ': # スペースキー
target_joint1 = 0.0
target_joint2 = 0.0
dt = 1.0
elif ord(c) == 27: # Escキー
break
commander.publish_joint1(target_joint1)
commander.publish_joint2(target_joint2)
# print("joint1",target_joint1,"joint2",target_joint2)
time.sleep(0.01)
except KeyboardInterrupt: # Ctrol-cでエラーにならないように
pass
rclpy.shutdown()
thread.join() # スレッドが終了するまで待つ
print('終了')
実行
- simple_arm_description
- ダウンロード：学内のみ可能です．
- https://kanazawa-it.box.com/s/izdowlwxq1o73lbbp3qn2exzroqv8bvi
- ダウンロードしたファイルは~/colcon_ws/srcに移動する．
- インストール
$ cd ~/colcon_ws/src$ unzip simple_arm-master.zip$ mv simple_arm-master simple_arm$ cd ~/colcon_ws$ source /opt/ros/foxy/setup.bash$ source install/setup.bash$ colcon build --symlink-install
- 実行
$ ros2 launch simple_arm_description display.launch.py
- ダウンロード：学内のみ可能です．
- armbot2
- パッケージの作成
- パッケージの作り方を思い出して，armbot2パッケージを作成する．
- コードの作成
- 上のソースコードをコピペして以下のファイルを作成する．
~/colcon_ws/src/armbot2/armbot2/commander1.py
- 上のソースコードをコピペして以下のファイルを作成する．
- 実行
$ cd ~/colcon_ws/src/armbot2/armbot2$ python3 commander1.py
- 遊び方
- １，２，３，４キーを押すと関節が回転してロボットアームが動く
- Spaceキー：初期姿勢
- パッケージの作成
終わり
コメント