39 lines
1.4 KiB
Python
39 lines
1.4 KiB
Python
"""模拟环境中的单周期轮询示例"""
|
|
|
|
from line_laser_modbus import DeviceStatus, LineLaserClient, ModeCommand, Pose6D, SerialConfig
|
|
from line_laser_modbus.runner import PollingRunner, pose_delta
|
|
from line_laser_modbus.simulator import SimulatedModbusBackend
|
|
|
|
|
|
def main() -> None:
|
|
"""演示轮询运行器如何读取快照并写入纠偏量"""
|
|
|
|
# 先让模拟控制器处于在线跟踪模式
|
|
# 只有 ONLINE_TRACKING 模式下 PollingRunner 才会写入纠偏寄存器
|
|
backend = SimulatedModbusBackend(
|
|
mode=ModeCommand.ONLINE_TRACKING,
|
|
status=DeviceStatus.TRACKING_OK,
|
|
current_pose=Pose6D(10.0, 20.0, 30.0, 0.0, 1.0, 2.0),
|
|
)
|
|
|
|
# 这里假设目标轨迹点是下面这个位姿
|
|
# pose_delta 会生成一个函数用于计算 目标位姿减当前位姿
|
|
target_pose = Pose6D(11.0, 22.0, 33.0, 0.0, 1.5, 1.0)
|
|
correction_provider = pose_delta(target_pose)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
runner = PollingRunner(client, correction_provider=correction_provider)
|
|
|
|
# run_once 只执行一个周期
|
|
# 测试和无硬件演示建议用它避免进入无限循环
|
|
snapshot = runner.run_once()
|
|
|
|
print("读取模式", snapshot.mode.name)
|
|
print("读取状态", snapshot.status.name)
|
|
print("读取当前位姿", snapshot.pose)
|
|
print("写入纠偏量", backend.correction())
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|