Pythonによる制御
ターミナル A で REXROV の例を起動したままにします。準備済みのターミナル Bで位置を読み、続いてスラスターに短い指令を送ります。
位置を購読する
次のプログラムを observe.py に保存し、ROS 環境を読み込んだターミナルで python3 observe.py を実行します。
import rclpy
from nav_msgs.msg import Odometry
from rclpy.node import Node
class Observer(Node):
def __init__(self):
super().__init__('posim_observer')
self.subscription = self.create_subscription(
Odometry, '/model/rexrov/odometry', self.on_pose, 10)
def on_pose(self, message):
p = message.pose.pose.position
self.get_logger().info(
f'x={p.x:.2f}, y={p.y:.2f}, z={p.z:.2f}', throttle_duration_sec=1.0)
rclpy.init()
node = Observer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
約 1 秒ごとに位置が表示されます。Ctrl+C は観測プログラムだけを停止し、シミュレーションは続きます。
時間を制限した指令を送る
次を pulse_thruster.py に保存し、python3 pulse_thruster.py を実行します。購読先を待ってから、ホスト側の時間で約 2 秒間、スラスター 1 に 5 N を指令し、最後にゼロを送ります。生じる力とトルクはスラスターの取り付け軸で決まります。
import time
import rclpy
from rclpy.node import Node
from rclpy.signals import SignalHandlerOptions
from std_msgs.msg import Float64
rclpy.init(signal_handler_options=SignalHandlerOptions.NO)
node = Node('posim_thruster_pulse')
publisher = node.create_publisher(
Float64, '/model/rexrov/joint/thruster1_joint/cmd_thrust', 10)
deadline = time.monotonic() + 10.0
try:
while publisher.get_subscription_count() == 0:
if time.monotonic() >= deadline:
raise RuntimeError('Start REXROV and load its ROS workspace first.')
rclpy.spin_once(node, timeout_sec=0.1)
print('Applying 5 N to thruster 1', flush=True)
for _ in range(20):
publisher.publish(Float64(data=5.0))
rclpy.spin_once(node, timeout_sec=0.1)
except KeyboardInterrupt:
pass
finally:
for _ in range(5):
publisher.publish(Float64(data=0.0))
rclpy.spin_once(node, timeout_sec=0.1)
print('Thruster command returned to zero', flush=True)
node.destroy_node()
rclpy.shutdown()
シグナルの設定により、Ctrl+C を押しても ROS の配信機能が残っている間に終了処理を実行できます。プロセスを強制終了した場合はシミュレーターを止めるか、スラスター制御に従って中立指令を送ってください。
応答を確認する
別の準備済みターミナルで、指令中の角速度を観測します。
ros2 topic echo --no-daemon /model/rexrov/joint/thruster1_joint/ang_vel std_msgs/msg/Float64
角速度が指令に応答し、終了後にゼロへ戻る様子を確認します。オドメトリも見ると、アクチュエーターの応答と機体全体の移動を区別できます。Ctrl+C で表示を止めます。
制御器を作る
計測状態と制御則に基づいて、REXROV の 8 基のスラスターへ力を配分します。トピック名、取り付け軸、単位をそろえてください。実験の処理にはシミュレーション時刻を使い、終了時には操作したすべてのアクチュエーターに中立値を送ります。
BlueROV2 は MAVROS/ArduSub を経由します。実験を比較するには観測データを記録してください。