使用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()
程序大约每秒输出一次位置。按 Ctrl+C 停止观测程序,仿真仍会继续。
发送有时间限制的指令
将以下程序保存为 pulse_thruster.py,运行 python3 pulse_thruster.py。程序等待订阅者出现,按主机时间向推进器 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 的八个推进器。保持话题名称、安装轴和单位一致。实验逻辑使用仿真时间;结束时向所有受控执行器发送中立值。
BlueROV2 使用 MAVROS/ArduSub 控制链路。需要比较实验时,可记录观测数据。