POSIM v1.0.1Platform for Ocean Simulation · ドキュメント
ドキュメントメニュー
POSIM / ロボット制御

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 を経由します。実験を比較するには観測データを記録してください。