347 lines
13 KiB
Python
Executable File
347 lines
13 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
"""Безопасно проехать на RMC по сетке до указанной ArUco-метки.
|
||
|
||
Скрипт использует штатные ROS 2 топики и TF. По умолчанию рассчитан на
|
||
физический робот; для Webots явно передайте --sim.
|
||
"""
|
||
|
||
import argparse
|
||
import math
|
||
import re
|
||
import time
|
||
from collections import deque
|
||
|
||
import rclpy
|
||
from geometry_msgs.msg import Twist
|
||
from nav_msgs.msg import Odometry
|
||
from rclpy.node import Node
|
||
from rclpy.parameter import Parameter
|
||
from rclpy.qos import qos_profile_sensor_data
|
||
from rclpy.time import Time
|
||
from sensor_msgs.msg import LaserScan
|
||
from std_msgs.msg import String
|
||
from tf2_ros import Buffer, TransformException, TransformListener
|
||
|
||
|
||
def clamp(value, low, high):
|
||
return max(low, min(high, value))
|
||
|
||
|
||
def wrap(angle):
|
||
return math.atan2(math.sin(angle), math.cos(angle))
|
||
|
||
|
||
def yaw(quaternion):
|
||
return math.atan2(
|
||
2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y),
|
||
1 - 2 * (quaternion.y**2 + quaternion.z**2),
|
||
)
|
||
|
||
|
||
def compose(a, b):
|
||
"""Перенести позу b из системы a в родительскую систему a."""
|
||
cosine, sine = math.cos(a[2]), math.sin(a[2])
|
||
return (
|
||
a[0] + cosine * b[0] - sine * b[1],
|
||
a[1] + sine * b[0] + cosine * b[1],
|
||
wrap(a[2] + b[2]),
|
||
)
|
||
|
||
|
||
def inverse(pose):
|
||
cosine, sine = math.cos(pose[2]), math.sin(pose[2])
|
||
return (
|
||
-cosine * pose[0] - sine * pose[1],
|
||
sine * pose[0] - cosine * pose[1],
|
||
-pose[2],
|
||
)
|
||
|
||
|
||
def marker_pose(marker, columns, spacing):
|
||
"""Штатная схема ЧВТ: 0=(0,0), 1=(0,+S), COLS=(-S,0)."""
|
||
row, column = divmod(marker, columns)
|
||
return -row * spacing, column * spacing, 0.0
|
||
|
||
|
||
def build_route(start, target, rows, columns, blocked):
|
||
queue = deque([start])
|
||
previous = {start: None}
|
||
|
||
while queue:
|
||
current = queue.popleft()
|
||
if current == target:
|
||
break
|
||
row, column = divmod(current, columns)
|
||
for next_row, next_column in (
|
||
(row - 1, column),
|
||
(row + 1, column),
|
||
(row, column - 1),
|
||
(row, column + 1),
|
||
):
|
||
candidate = next_row * columns + next_column
|
||
if not (0 <= next_row < rows and 0 <= next_column < columns):
|
||
continue
|
||
if candidate in blocked or candidate in previous:
|
||
continue
|
||
previous[candidate] = current
|
||
queue.append(candidate)
|
||
|
||
if target not in previous:
|
||
raise RuntimeError(f"Нет маршрута {start} -> {target}")
|
||
route = []
|
||
current = target
|
||
while current is not None:
|
||
route.append(current)
|
||
current = previous[current]
|
||
return list(reversed(route))
|
||
|
||
|
||
class ArucoDrive(Node):
|
||
def __init__(self, args):
|
||
super().__init__("mvch_basic_aruco_drive")
|
||
self.args = args
|
||
self.robot = args.robot.strip("/")
|
||
if args.sim:
|
||
self.set_parameters([Parameter("use_sim_time", value=True)])
|
||
|
||
self.odom = None
|
||
self.pose = None
|
||
self.map_from_odom = None
|
||
self.marker = None
|
||
self.marker_error = math.inf
|
||
self.front = None
|
||
self.odom_wall = 0.0
|
||
self.marker_wall = 0.0
|
||
self.scan_wall = 0.0
|
||
|
||
self.odom_frame = f"{self.robot}/odom"
|
||
self.tf_buffer = Buffer()
|
||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||
self.publisher = self.create_publisher(
|
||
Twist, f"/{self.robot}/cmd_vel", 10
|
||
)
|
||
self.create_subscription(
|
||
Odometry,
|
||
f"/{self.robot}/odometry",
|
||
self.on_odometry,
|
||
qos_profile_sensor_data,
|
||
)
|
||
self.create_subscription(
|
||
LaserScan,
|
||
args.scan_topic or f"/{self.robot}/scan_front",
|
||
self.on_scan,
|
||
qos_profile_sensor_data,
|
||
)
|
||
self.create_subscription(
|
||
String, f"/{self.robot}/aruco_id", self.on_marker, 10
|
||
)
|
||
|
||
def on_odometry(self, message):
|
||
pose = message.pose.pose
|
||
values = (pose.position.x, pose.position.y, yaw(pose.orientation))
|
||
if all(math.isfinite(value) for value in values):
|
||
self.odom = values
|
||
self.odom_wall = time.monotonic()
|
||
|
||
def on_marker(self, message):
|
||
match = re.fullmatch(r"(?:aruco_)?(\d+)", message.data.strip())
|
||
if match:
|
||
marker = int(match.group(1))
|
||
if 0 <= marker < self.args.rows * self.args.columns:
|
||
self.marker = marker
|
||
self.marker_wall = time.monotonic()
|
||
|
||
def on_scan(self, message):
|
||
if not message.ranges or message.angle_increment == 0:
|
||
return
|
||
distances = []
|
||
for index, distance in enumerate(message.ranges):
|
||
angle = message.angle_min + index * message.angle_increment
|
||
if abs(angle) > math.radians(25):
|
||
continue
|
||
if math.isfinite(distance) and message.range_min <= distance <= message.range_max:
|
||
distances.append(distance)
|
||
# Пустое пространство lidar часто кодирует как inf. Это свежий скан,
|
||
# просто без препятствий в выбранном секторе.
|
||
self.front = min(distances, default=math.inf)
|
||
self.scan_wall = time.monotonic()
|
||
|
||
def localize(self):
|
||
"""Привязать odometry к сетке по последней видимой метке."""
|
||
if self.odom is None or self.marker is None:
|
||
return
|
||
try:
|
||
transform = self.tf_buffer.lookup_transform(
|
||
self.odom_frame, f"aruco_{self.marker}", Time()
|
||
).transform
|
||
except TransformException:
|
||
return
|
||
|
||
odom_marker = (
|
||
transform.translation.x,
|
||
transform.translation.y,
|
||
yaw(transform.rotation),
|
||
)
|
||
correction = compose(
|
||
marker_pose(self.marker, self.args.columns, self.args.spacing),
|
||
inverse(odom_marker),
|
||
)
|
||
new_pose = compose(correction, self.odom)
|
||
if self.pose is not None:
|
||
jump = math.hypot(new_pose[0] - self.pose[0], new_pose[1] - self.pose[1])
|
||
if jump > 0.35:
|
||
return
|
||
self.map_from_odom = correction
|
||
self.pose = new_pose
|
||
self.marker_error = math.hypot(
|
||
odom_marker[0] - self.odom[0], odom_marker[1] - self.odom[1]
|
||
)
|
||
|
||
def update(self):
|
||
rclpy.spin_once(self, timeout_sec=0.05)
|
||
self.localize()
|
||
if self.map_from_odom is not None and self.odom is not None:
|
||
self.pose = compose(self.map_from_odom, self.odom)
|
||
|
||
def stop(self):
|
||
self.publisher.publish(Twist())
|
||
|
||
def wait_until_ready(self):
|
||
print("Жду odometry, scan_front, aruco_id и TF метки...")
|
||
deadline = time.monotonic() + self.args.ready_timeout
|
||
while rclpy.ok() and time.monotonic() < deadline:
|
||
self.update()
|
||
self.stop()
|
||
now = time.monotonic()
|
||
if (
|
||
self.pose is not None
|
||
and self.front is not None
|
||
and now - self.odom_wall < 1.0
|
||
and now - self.scan_wall < 1.0
|
||
and now - self.marker_wall < 1.0
|
||
):
|
||
return
|
||
raise RuntimeError(
|
||
"Датчики не готовы. Проверьте ros2 topic list, aruco_id и TF aruco_<ID>."
|
||
)
|
||
|
||
def check_safety(self):
|
||
now = time.monotonic()
|
||
if now - self.odom_wall > 1.0:
|
||
raise RuntimeError("Потеря odometry")
|
||
if now - self.scan_wall > 1.0:
|
||
raise RuntimeError("Потеря scan_front")
|
||
if self.front < self.args.stop_distance:
|
||
raise RuntimeError(
|
||
f"Препятствие спереди: {self.front:.2f} м "
|
||
f"(< {self.args.stop_distance:.2f} м)"
|
||
)
|
||
|
||
def drive_to(self, target):
|
||
target_pose = marker_pose(target, self.args.columns, self.args.spacing)
|
||
deadline = time.monotonic() + self.args.waypoint_timeout
|
||
|
||
while rclpy.ok() and time.monotonic() < deadline:
|
||
self.update()
|
||
self.check_safety()
|
||
dx = target_pose[0] - self.pose[0]
|
||
dy = target_pose[1] - self.pose[1]
|
||
distance = math.hypot(dx, dy)
|
||
marker_is_fresh = (
|
||
self.marker == target
|
||
and time.monotonic() - self.marker_wall < 1.0
|
||
and self.marker_error <= self.args.tolerance
|
||
)
|
||
if distance <= self.args.tolerance and marker_is_fresh:
|
||
self.stop()
|
||
print(f"Метка {target} достигнута, ошибка {self.marker_error:.3f} м")
|
||
return
|
||
|
||
angle_error = wrap(math.atan2(dy, dx) - self.pose[2])
|
||
command = Twist()
|
||
if abs(angle_error) > 0.15:
|
||
command.angular.z = clamp(
|
||
1.4 * angle_error,
|
||
-self.args.max_angular,
|
||
self.args.max_angular,
|
||
)
|
||
else:
|
||
command.linear.x = min(self.args.max_speed, 0.5 * distance)
|
||
command.angular.z = clamp(
|
||
1.2 * angle_error,
|
||
-self.args.max_angular,
|
||
self.args.max_angular,
|
||
)
|
||
self.publisher.publish(command)
|
||
|
||
raise RuntimeError(f"Таймаут движения к метке {target}")
|
||
|
||
|
||
def arguments():
|
||
parser = argparse.ArgumentParser(description=__doc__)
|
||
parser.add_argument("target", type=int, help="ID целевой ArUco")
|
||
parser.add_argument("--robot", default="RMC2", help="RMC1 или RMC2")
|
||
parser.add_argument("--rows", type=int, default=6)
|
||
parser.add_argument("--columns", type=int, default=6)
|
||
parser.add_argument("--spacing", type=float, default=1.0, help="Шаг сетки, м")
|
||
parser.add_argument("--blocked", type=int, nargs="*", default=[])
|
||
parser.add_argument("--max-speed", type=float, default=0.12, help="м/с")
|
||
parser.add_argument("--max-angular", type=float, default=0.30, help="рад/с")
|
||
parser.add_argument("--stop-distance", type=float, default=0.55, help="м")
|
||
parser.add_argument("--tolerance", type=float, default=0.06, help="м")
|
||
parser.add_argument("--ready-timeout", type=float, default=15.0, help="с")
|
||
parser.add_argument("--waypoint-timeout", type=float, default=45.0, help="с")
|
||
parser.add_argument("--scan-topic", help="Например /RMC2/scan_front")
|
||
parser.add_argument("--yes", action="store_true", help="Не ждать Enter")
|
||
parser.add_argument("--sim", action="store_true", help="Использовать /clock Webots")
|
||
args = parser.parse_args()
|
||
count = args.rows * args.columns
|
||
if args.rows <= 0 or args.columns <= 0 or not 0 <= args.target < count:
|
||
parser.error(f"target должен быть в диапазоне 0..{count - 1}")
|
||
if args.spacing <= 0 or args.max_speed <= 0 or args.max_angular <= 0:
|
||
parser.error("spacing и скорости должны быть больше нуля")
|
||
return args
|
||
|
||
|
||
def main():
|
||
args = arguments()
|
||
rclpy.init()
|
||
node = ArucoDrive(args)
|
||
exit_code = 0
|
||
try:
|
||
node.wait_until_ready()
|
||
start = node.marker
|
||
blocked = set(args.blocked) - {start, args.target}
|
||
route = build_route(
|
||
start, args.target, args.rows, args.columns, blocked
|
||
)
|
||
print(f"Стартовая метка: {start}")
|
||
print("Маршрут:", " -> ".join(map(str, route)))
|
||
print(
|
||
f"Топики: /{node.robot}/cmd_vel, /{node.robot}/odometry, "
|
||
f"/{node.robot}/aruco_id, {args.scan_topic or f'/{node.robot}/scan_front'}"
|
||
)
|
||
if not args.yes:
|
||
input("Проверьте поле и держите аппаратный STOP. Enter — начать: ")
|
||
# Во время input ROS не обрабатывал сообщения: перед движением снова
|
||
# получить свежие odometry/lidar/ArUco.
|
||
node.wait_until_ready()
|
||
for marker in route[1:]:
|
||
node.drive_to(marker)
|
||
node.stop()
|
||
print(f"FINISHED: ровер стоит над ArUco {args.target}")
|
||
except (KeyboardInterrupt, RuntimeError) as error:
|
||
exit_code = 1
|
||
print(f"STOP: {error}")
|
||
finally:
|
||
for _ in range(10):
|
||
node.stop()
|
||
rclpy.spin_once(node, timeout_sec=0.02)
|
||
node.destroy_node()
|
||
rclpy.shutdown()
|
||
raise SystemExit(exit_code)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|