66 lines
2.1 KiB
Python
66 lines
2.1 KiB
Python
#!/usr/bin/env python3
|
|
"""Поднимает или опускает лифт RMC2 через штатный ROS-топик."""
|
|
|
|
import argparse
|
|
import time
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
|
|
from std_msgs.msg import Float64, String
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("state", choices=("up", "down"))
|
|
parser.add_argument("--wait", type=float, default=6.0, help="Сколько ждать статус, с")
|
|
args = parser.parse_args()
|
|
|
|
rclpy.init()
|
|
node = Node("mvch_basic_lift")
|
|
exit_message = None
|
|
try:
|
|
publisher = node.create_publisher(Float64, "/RMC2/lift", 10)
|
|
statuses = []
|
|
status_qos = QoSProfile(
|
|
depth=1,
|
|
reliability=ReliabilityPolicy.RELIABLE,
|
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
|
)
|
|
node.create_subscription(
|
|
String,
|
|
"/RMC2/lift_status",
|
|
lambda msg: statuses.append(msg.data),
|
|
status_qos,
|
|
)
|
|
|
|
height = 0.05 if args.state == "up" else 0.0
|
|
expected = "raised" if args.state == "up" else "lowered"
|
|
print(f"RMC2 lift: {args.state}, команда {height}")
|
|
for _ in range(5):
|
|
publisher.publish(Float64(data=height))
|
|
rclpy.spin_once(node, timeout_sec=0.1)
|
|
|
|
deadline = time.monotonic() + args.wait
|
|
while time.monotonic() < deadline:
|
|
rclpy.spin_once(node, timeout_sec=0.1)
|
|
if statuses and statuses[-1].strip().lower() == expected:
|
|
break
|
|
|
|
if statuses and statuses[-1].strip().lower() == expected:
|
|
print(f"Статус: {statuses[-1]}")
|
|
else:
|
|
current = statuses[-1] if statuses else "нет данных"
|
|
exit_message = (
|
|
f"Лифт не подтвердил {expected}; текущий статус: {current}"
|
|
)
|
|
finally:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
if exit_message:
|
|
raise SystemExit(exit_message)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|