94 lines
3.7 KiB
Python
Executable File
94 lines
3.7 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""Подаёт ограниченную по времени ROS-команду скорости и затем останавливает RMC."""
|
|
|
|
import argparse
|
|
import math
|
|
import time
|
|
|
|
import rclpy
|
|
from geometry_msgs.msg import Twist
|
|
from rclpy.node import Node
|
|
from rclpy.qos import qos_profile_sensor_data
|
|
from sensor_msgs.msg import LaserScan
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--robot", choices=("RMC1", "RMC2"), required=True)
|
|
parser.add_argument("--x", type=float, default=0.0, help="Скорость вперёд, м/с")
|
|
parser.add_argument("--y", type=float, default=0.0, help="Боковая скорость RMC1, м/с")
|
|
parser.add_argument("--yaw", type=float, default=0.0, help="Угловая скорость, рад/с")
|
|
parser.add_argument("--seconds", type=float, default=1.0)
|
|
parser.add_argument("--stop-distance", type=float, default=0.55, help="Стоп по lidar, м")
|
|
parser.add_argument("--no-lidar", action="store_true", help="Только стенд без препятствий")
|
|
args = parser.parse_args()
|
|
|
|
if args.seconds <= 0:
|
|
parser.error("--seconds должен быть больше нуля")
|
|
if args.robot == "RMC2" and args.y != 0:
|
|
parser.error("RMC2 не поддерживает боковое движение: используйте --y 0")
|
|
|
|
rclpy.init()
|
|
node = Node("mvch_basic_move")
|
|
publisher = node.create_publisher(Twist, f"/{args.robot}/cmd_vel", 10)
|
|
front_distance = [None]
|
|
|
|
def on_scan(message):
|
|
if not message.ranges or message.angle_increment == 0:
|
|
return
|
|
front = []
|
|
for index, distance in enumerate(message.ranges):
|
|
angle = message.angle_min + index * message.angle_increment
|
|
if abs(angle) <= 0.44 and math.isfinite(distance):
|
|
if message.range_min <= distance <= message.range_max:
|
|
front.append(distance)
|
|
front_distance[0] = min(front, default=math.inf)
|
|
|
|
if not args.no_lidar:
|
|
node.create_subscription(
|
|
LaserScan,
|
|
f"/{args.robot}/scan_front",
|
|
on_scan,
|
|
qos_profile_sensor_data,
|
|
)
|
|
command = Twist()
|
|
command.linear.x = args.x
|
|
command.linear.y = args.y
|
|
command.angular.z = args.yaw
|
|
|
|
print(f"{args.robot}: cmd_vel x={args.x}, y={args.y}, yaw={args.yaw}")
|
|
if not args.no_lidar:
|
|
print(f"Жду /{args.robot}/scan_front...")
|
|
deadline = time.monotonic() + 3.0
|
|
while front_distance[0] is None and time.monotonic() < deadline:
|
|
rclpy.spin_once(node, timeout_sec=0.1)
|
|
if front_distance[0] is None:
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
raise SystemExit("Нет scan_front: движение запрещено. Для стенда: --no-lidar")
|
|
started = time.monotonic()
|
|
exit_code = 0
|
|
try:
|
|
while time.monotonic() - started < args.seconds:
|
|
if front_distance[0] is not None and front_distance[0] < args.stop_distance:
|
|
raise RuntimeError(
|
|
f"Препятствие {front_distance[0]:.2f} м — аварийный стоп"
|
|
)
|
|
publisher.publish(command)
|
|
rclpy.spin_once(node, timeout_sec=0.05)
|
|
except RuntimeError as error:
|
|
exit_code = 1
|
|
print(f"STOP: {error}")
|
|
finally:
|
|
stop = Twist()
|
|
for _ in range(5):
|
|
publisher.publish(stop)
|
|
rclpy.spin_once(node, timeout_sec=0.05)
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
raise SystemExit(exit_code)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|