feat: add basic ROS rover scripts
This commit is contained in:
@@ -0,0 +1,86 @@
|
||||
# Базовые примеры для реального ROS 2
|
||||
|
||||
Это короткие учебные скрипты, которые работают через штатные ROS 2 интерфейсы
|
||||
роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. Имена
|
||||
сверены с выданной документацией физических РМК от 08.09.2026. В них нет
|
||||
прямого управления моторами и нет подмены симулятора. Один и тот же ROS API можно
|
||||
проверять в Webots, а затем перенести на физический РМК после сверки имён топиков,
|
||||
TF, размеров поля и безопасных скоростей.
|
||||
|
||||
Это не готовые конкурсные решения и не замена аппаратному аварийному STOP.
|
||||
|
||||
Перед запуском:
|
||||
|
||||
```bash
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
|
||||
```
|
||||
|
||||
Сначала проверьте реальные интерфейсы:
|
||||
|
||||
```bash
|
||||
ros2 topic list | sort
|
||||
ros2 topic info /RMC2/cmd_vel -v
|
||||
ros2 topic info /RMC2/odometry -v
|
||||
ros2 topic info /RMC2/scan_front -v
|
||||
ros2 topic info /RMC2/camera_bottom/aruco_id -v
|
||||
ros2 topic echo /RMC2/camera_bottom/aruco_id --once
|
||||
ros2 run tf2_ros tf2_echo RMC2/odom aruco_0
|
||||
```
|
||||
|
||||
Практические примеры:
|
||||
|
||||
```bash
|
||||
# Проехать RMC2 от текущей метки до ArUco 14 на физическом ROS
|
||||
python3 basic/drive_to_aruco.py 14
|
||||
|
||||
# То же в Webots; только здесь нужен --sim
|
||||
python3 basic/drive_to_aruco.py 14 --sim
|
||||
|
||||
# Равнозначная форма с именованным аргументом
|
||||
python3 basic/drive_to_aruco.py --sim --target 14
|
||||
|
||||
# Закрытые клетки и уменьшенная реальная скорость
|
||||
python3 basic/drive_to_aruco.py 14 --blocked 8 9 --max-speed 0.08
|
||||
|
||||
# Короткая проверка движения; lidar обязателен, после 1 с будет stop
|
||||
python3 basic/move_rmc.py --robot RMC1 --x 0.15 --seconds 1
|
||||
|
||||
# Поворот RMC2 на месте
|
||||
python3 basic/move_rmc.py --robot RMC2 --yaw 0.3 --seconds 1
|
||||
|
||||
# Статус положения и лидара
|
||||
python3 basic/watch_robot.py --robot RMC1
|
||||
|
||||
# Управление лифтом RMC2
|
||||
python3 basic/lift_rmc2.py up
|
||||
python3 basic/lift_rmc2.py down
|
||||
|
||||
# Остановить оба ровера
|
||||
python3 basic/stop_all.py
|
||||
```
|
||||
|
||||
`drive_to_aruco.py` по умолчанию использует реальную сетку 5×5: `0=(0,0)`,
|
||||
`1=(0,+1 м)`, `5=(-1 м,0)`. С `--sim` выбирается сетка Webots 6×6, где
|
||||
`6=(-1 м,0)`. Ровер должен стартовать центром над видимой меткой.
|
||||
Скрипт сам определяет старт, строит BFS-маршрут, печатает его до движения, затем
|
||||
едет по odometry и уточняет положение через TF каждой ArUco. Без свежих odometry,
|
||||
лидара, метки на финише или при препятствии ближе `--stop-distance` он отправляет
|
||||
нулевой `Twist` и завершает работу.
|
||||
|
||||
На реальном поле сначала измерьте шаг сетки и при необходимости задайте
|
||||
`--spacing`. Если организаторы используют другой топик лидара, передайте
|
||||
`--scan-topic /нужный/топик`. Не используйте `--yes` в первом физическом заезде.
|
||||
|
||||
По выданной документации нижняя камера и `aruco_id` есть у RMC2. Поэтому реальный
|
||||
запуск `drive_to_aruco.py` разрешён только для RMC2. Подъём лифта публикует `0.05`,
|
||||
опускание — `0.0`, а статус читается с обязательным QoS `Transient Local`.
|
||||
|
||||
У RMC1 доступно боковое движение (`linear.y`), у RMC2 оно принудительно запрещено
|
||||
в `move_rmc.py`. Режим `--no-lidar` предназначен только для вывешенного стенда,
|
||||
где колёса не могут переместить робот.
|
||||
|
||||
`grid_route.py` оставлен как офлайн-проверка графа. Он ничего не публикует в ROS.
|
||||
Для реального поля используется его дефолт `--size 5`, для Webots добавьте
|
||||
`--size 6`.
|
||||
@@ -0,0 +1,409 @@
|
||||
#!/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 (
|
||||
DurabilityPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from rclpy.time import Time
|
||||
from sensor_msgs.msg import LaserScan
|
||||
from std_msgs.msg import String
|
||||
from tf2_msgs.msg import TFMessage
|
||||
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)
|
||||
if not args.sim:
|
||||
tf_qos = QoSProfile(
|
||||
depth=100,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
durability=DurabilityPolicy.VOLATILE,
|
||||
)
|
||||
static_tf_qos = QoSProfile(
|
||||
depth=100,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
self.create_subscription(
|
||||
TFMessage, f"/{self.robot}/tf", self.on_tf, tf_qos
|
||||
)
|
||||
self.create_subscription(
|
||||
TFMessage,
|
||||
f"/{self.robot}/tf_static",
|
||||
self.on_tf_static,
|
||||
static_tf_qos,
|
||||
)
|
||||
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.aruco_topic = (
|
||||
f"/{self.robot}/aruco_id"
|
||||
if args.sim
|
||||
else f"/{self.robot}/camera_bottom/aruco_id"
|
||||
)
|
||||
self.create_subscription(String, self.aruco_topic, self.on_marker, 10)
|
||||
|
||||
def on_tf(self, message):
|
||||
for transform in message.transforms:
|
||||
self.tf_buffer.set_transform(transform, f"{self.robot} namespaced tf")
|
||||
|
||||
def on_tf_static(self, message):
|
||||
for transform in message.transforms:
|
||||
self.tf_buffer.set_transform_static(
|
||||
transform, f"{self.robot} namespaced tf_static"
|
||||
)
|
||||
|
||||
def on_odometry(self, message):
|
||||
frame = message.header.frame_id.strip("/")
|
||||
if frame:
|
||||
self.odom_frame = frame
|
||||
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", nargs="?", type=int, help="ID целевой ArUco")
|
||||
parser.add_argument(
|
||||
"--target",
|
||||
dest="target_option",
|
||||
type=int,
|
||||
help="ID целевой ArUco (альтернатива позиционному аргументу)",
|
||||
)
|
||||
parser.add_argument("--robot", choices=("RMC1", "RMC2"), default="RMC2")
|
||||
parser.add_argument("--rows", type=int)
|
||||
parser.add_argument("--columns", type=int)
|
||||
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()
|
||||
if args.target is not None and args.target_option is not None:
|
||||
parser.error("укажите target один раз: позиционно или через --target")
|
||||
if args.target is None:
|
||||
args.target = args.target_option
|
||||
if args.target is None:
|
||||
parser.error("укажите ID цели: TARGET или --target TARGET")
|
||||
if args.rows is None:
|
||||
args.rows = 6 if args.sim else 5
|
||||
if args.columns is None:
|
||||
args.columns = 6 if args.sim else 5
|
||||
if not args.sim and args.robot != "RMC2":
|
||||
parser.error(
|
||||
"Документация физического RMC1 не содержит нижнюю ArUco-камеру; "
|
||||
"drive_to_aruco поддерживает реальный RMC2"
|
||||
)
|
||||
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.aruco_topic}, {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()
|
||||
@@ -0,0 +1,69 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Строит кратчайший маршрут по регулярной сетке ArUco без ROS."""
|
||||
|
||||
import argparse
|
||||
from collections import deque
|
||||
|
||||
|
||||
def neighbors(marker: int, size: int):
|
||||
row, column = divmod(marker, size)
|
||||
for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)):
|
||||
next_row, next_column = row + dr, column + dc
|
||||
if 0 <= next_row < size and 0 <= next_column < size:
|
||||
yield next_row * size + next_column
|
||||
|
||||
|
||||
def build_route(start: int, target: int, blocked: set[int], size: int):
|
||||
queue = deque([start])
|
||||
previous = {start: None}
|
||||
|
||||
while queue:
|
||||
current = queue.popleft()
|
||||
if current == target:
|
||||
break
|
||||
for candidate in neighbors(current, size):
|
||||
if candidate not in blocked and candidate not in previous:
|
||||
previous[candidate] = current
|
||||
queue.append(candidate)
|
||||
|
||||
if target not in previous:
|
||||
return None
|
||||
|
||||
route = []
|
||||
current = target
|
||||
while current is not None:
|
||||
route.append(current)
|
||||
current = previous[current]
|
||||
return list(reversed(route))
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("start", type=int, help="ID стартового маркера")
|
||||
parser.add_argument("target", type=int, help="ID целевого маркера")
|
||||
parser.add_argument("--size", type=int, default=5, help="5 для реального поля, 6 для Webots")
|
||||
parser.add_argument(
|
||||
"--blocked",
|
||||
type=int,
|
||||
nargs="*",
|
||||
default=[],
|
||||
help="Маркеры, через которые нельзя ехать",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.size < 2:
|
||||
parser.error("--size должен быть не меньше 2")
|
||||
if not 0 <= args.start < args.size**2 or not 0 <= args.target < args.size**2:
|
||||
parser.error(f"start и target должны быть от 0 до {args.size**2 - 1}")
|
||||
|
||||
blocked = set(args.blocked)
|
||||
blocked.discard(args.start)
|
||||
blocked.discard(args.target)
|
||||
route = build_route(args.start, args.target, blocked, args.size)
|
||||
if route is None:
|
||||
raise SystemExit("Маршрут не найден")
|
||||
print(" -> ".join(map(str, route)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,65 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,93 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,31 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Публикует нулевую скорость обоим роверам."""
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = Node("mvch_basic_stop_all")
|
||||
publishers = [
|
||||
node.create_publisher(Twist, "/RMC1/cmd_vel", 10),
|
||||
node.create_publisher(Twist, "/RMC2/cmd_vel", 10),
|
||||
]
|
||||
stop = Twist()
|
||||
for _ in range(10):
|
||||
for publisher in publishers:
|
||||
publisher.publish(stop)
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
time.sleep(0.02)
|
||||
print("Нулевая скорость отправлена RMC1 и RMC2")
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Показывает одометрию и минимальную дистанцию перед ровером."""
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from nav_msgs.msg import Odometry
|
||||
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)
|
||||
args = parser.parse_args()
|
||||
|
||||
rclpy.init()
|
||||
node = Node("mvch_basic_watch")
|
||||
state = {"pose": None, "front": None}
|
||||
|
||||
def on_odom(message):
|
||||
position = message.pose.pose.position
|
||||
orientation = message.pose.pose.orientation
|
||||
siny = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
|
||||
cosy = 1.0 - 2.0 * (orientation.y**2 + orientation.z**2)
|
||||
state["pose"] = (position.x, position.y, math.atan2(siny, cosy))
|
||||
|
||||
def on_scan(message):
|
||||
front = []
|
||||
for index, distance in enumerate(message.ranges):
|
||||
angle = message.angle_min + index * message.angle_increment
|
||||
if abs(angle) <= math.radians(20) and math.isfinite(distance):
|
||||
if message.range_min <= distance <= message.range_max:
|
||||
front.append(distance)
|
||||
state["front"] = min(front, default=math.inf)
|
||||
|
||||
node.create_subscription(
|
||||
Odometry, f"/{args.robot}/odometry", on_odom, qos_profile_sensor_data
|
||||
)
|
||||
node.create_subscription(
|
||||
LaserScan, f"/{args.robot}/scan_front", on_scan, qos_profile_sensor_data
|
||||
)
|
||||
|
||||
print(f"Слушаю {args.robot}. Ctrl+C — выход.")
|
||||
last_print = 0.0
|
||||
try:
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(node, timeout_sec=0.1)
|
||||
if time.monotonic() - last_print >= 0.5:
|
||||
pose = state["pose"]
|
||||
front = state["front"]
|
||||
pose_text = "нет odometry" if pose is None else (
|
||||
f"x={pose[0]:.2f}, y={pose[1]:.2f}, yaw={math.degrees(pose[2]):.1f}°"
|
||||
)
|
||||
front_text = "нет scan_front" if front is None else f"{front:.2f} м"
|
||||
print(f"{pose_text}; спереди: {front_text}")
|
||||
last_print = time.monotonic()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user