Хабр Курсы для всех
РЕКЛАМА
Большая витрина: от крупнейших школ до частных авторов. Сравнивайте по цене, длительности, формату и выбирайте самый подходящий курс!
Поток DIY доступен 24/7 благодаря поддержке друзей Хабра
Комментарии 7
Вот скажите Владимир, этот код который вы выложили, вы хотя-бы в симуляторе проверили? Или на вашем самодельном манипуляторе? Если да, почему не добавить в статью gif анимации/видео как оно работает. Если нет и это просто сгенерированный LLM код, оторванный от реальности не выкладывайте его, LLM сочинят всякий бред который зачастую выглядит очень правдоподобно.
Добрый день!
Тестировали в симуляторе, работает. Немного позже приложу доказательство. На самом деле код очень простой и это скорее начало пути для управления манипулятором, в будущих статьях покажем работу и в симуляторе, и на реальном прототипе в теплице. Решение разработано несколько месяцев назад, только сейчас руки доходят начинать описывать.
В качестве дополнения можно создать простой интерфейс, используя какой-нибудь фреймворк интерфейса и ROS2. Конкретно тут покажу, как взаимодействовать с элементами интерфейса, получая данные из топиков.
1. Подключение библиотек, данные роботов и запуск ROS2
В первой части подключаются необходимые библиотеки и создаётся структура, в которой будут храниться последние данные от двух роботов. Для каждого РМК сохраняются координаты x, y, угол поворота yaw, напряжение батареи и последнее сообщение лидара.
После этого инициализируется ROS 2 и создаётся узел fms_visualizer. Функция quaternion_to_yaw() нужна потому, что ориентация робота в сообщении одометрии приходит в виде кватерниона, а для отображения на плоской карте удобнее использовать обычный угол поворота.
import tkinter as tk
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float32
GRID_SIZE = 5
CELL_SIZE = 100
MAP_OFFSET = 50
robots = {
"РМК-1": {
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"battery": None,
"scan": None
},
"РМК-2": {
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"battery": None,
"scan": None
}
}
rclpy.init()
node = Node("fms_visualizer")
def quaternion_to_yaw(q):
sin_yaw = 2.0 * (q.w * q.z + q.x * q.y)
cos_yaw = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
return math.atan2(sin_yaw, cos_yaw)Таким образом, на этом этапе у программы уже есть ROS-узел и место, куда в дальнейшем будут записываться данные от каждого робота.
2. Получение одометрии, лидара и состояния батареи
Следующая часть отвечает за получение информации из ROS2.
Функция update_odometry() сохраняет положение и ориентацию выбранного робота. Для каждого РМК сделаны отдельные callback-функции, поскольку данные приходят из разных топиков.
Аналогично принимаются данные лидара и напряжение аккумулятора.
def update_odometry(name, msg):
robots[name]["x"] = msg.pose.pose.position.x
robots[name]["y"] = msg.pose.pose.position.y
robots[name]["yaw"] = quaternion_to_yaw(msg.pose.pose.orientation)
def odom_rmc1(msg):
update_odometry("РМК-1", msg)
def odom_rmc2(msg):
update_odometry("РМК-2", msg)
def battery_rmc1(msg):
robots["РМК-1"]["battery"] = msg.data
def battery_rmc2(msg):
robots["РМК-2"]["battery"] = msg.data
def scan_rmc1(msg):
robots["РМК-1"]["scan"] = msg
def scan_rmc2(msg):
robots["РМК-2"]["scan"] = msg
node.create_subscription(Odometry, "/RMC1/odometry", odom_rmc1, 10)
node.create_subscription(LaserScan, "/RMC1/scan", scan_rmc1, qos_profile_sensor_data)
node.create_subscription(Float32, "/RMC1/odrive_voltage", battery_rmc1, 10)
node.create_subscription(Odometry, "/RMC2/odometry", odom_rmc2, 10)
node.create_subscription(LaserScan, "/RMC2/scan", scan_rmc2, qos_profile_sensor_data)
node.create_subscription(Float32, "/RMC2/odrive_voltage", battery_rmc2, 10)В результате визуализатор одновременно подписан на шесть топиков:
При этом интерфейсу не нужно постоянно обращаться к ROS-топикам напрямую. Callback-функции обновляют словарь robots, а графическая часть программы уже берёт актуальные значения из него.
3. Отрисовка робота и данных лидара
Теперь можно перейти к визуализации полученных данных. В этой части покажу как это можно сделать с canvas tkinter.
Робот отображается кругом, а его направление — стрелкой. Координаты из ROS сначала преобразуются в координаты Canvas.
Для лидара выполняется дополнительное преобразование. Каждый луч сначала задан относительно самого робота. Поэтому координаты точки необходимо повернуть на текущий yaw робота и перенести в мировую систему координат.
def draw_robot(name):
x = robots[name]["x"]
y = robots[name]["y"]
yaw = robots[name]["yaw"]
canvas_x, canvas_y = world_to_canvas(x, y)
radius = 15
canvas.coords(
robot_body,
canvas_x - radius,
canvas_y - radius,
canvas_x + radius,
canvas_y + radius
)
arrow_length = 35
arrow_x = canvas_x + arrow_length * math.cos(yaw)
arrow_y = canvas_y - arrow_length * math.sin(yaw)
canvas.coords(
robot_arrow,
canvas_x,
canvas_y,
arrow_x,
arrow_y
)
def draw_lidar(name):
canvas.delete("lidar")
scan = robots[name]["scan"]
if scan is None:
return
robot_x = robots[name]["x"]
robot_y = robots[name]["y"]
robot_yaw = robots[name]["yaw"]
for i in range(0, len(scan.ranges), 2):
distance = scan.ranges[i]
if not math.isfinite(distance):
continue
if distance < scan.range_min:
continue
if distance > scan.range_max:
continue
angle = scan.angle_min + i * scan.angle_increment
local_x = distance * math.cos(angle)
local_y = distance * math.sin(angle)
world_x = (
robot_x
+ local_x * math.cos(robot_yaw)
- local_y * math.sin(robot_yaw)
)
world_y = (
robot_y
+ local_x * math.sin(robot_yaw)
+ local_y * math.cos(robot_yaw)
)
canvas_x, canvas_y = world_to_canvas(world_x, world_y)
radius = 2
canvas.create_oval(
canvas_x - radius,
canvas_y - radius,
canvas_x + radius,
canvas_y + radius,
fill="red",
outline="red",
tags="lidar"
)Для уменьшения количества отображаемых точек берётся каждый второй луч:
for i in range(0, len(scan.ranges), 2):Значения inf, nan, а также измерения за пределами рабочего диапазона лидара отбрасываются.
4. Обновление интерфейса и создание окна
Следующая часть связывает данные ROS с Tkinter.
update_gui() определяет выбранного пользователем робота, выводит его координаты, угол и напряжение батареи, а затем обновляет изображение робота и лидара.
Отдельная функция ros_spin() периодически вызывает rclpy.spin_once(). Это позволяет обрабатывать ROS-сообщения и одновременно не блокировать главный цикл Tkinter.
def update_gui():
name = selected_robot.get()
x = robots[name]["x"]
y = robots[name]["y"]
yaw = robots[name]["yaw"]
yaw_degrees = math.degrees(yaw)
position_label.config(
text=(
f"{name}\n\n"
f"Одометрия:\n"
f"x = {x:.2f} м\n"
f"y = {y:.2f} м\n"
f"yaw = {yaw_degrees:.1f}°"
)
)
battery = robots[name]["battery"]
if battery is None:
battery_label.config(text="Батарея: нет данных")
else:
battery_label.config(text=f"Батарея: {battery:.2f} V")
draw_lidar(name)
draw_robot(name)
root.after(100, update_gui)
def ros_spin():
rclpy.spin_once(node, timeout_sec=0)
root.after(10, ros_spin)
def close_program():
node.destroy_node()
rclpy.shutdown()
root.destroy()
root = tk.Tk()
root.title("FMS — визуализация РМК")
root.geometry("900x700")
canvas = tk.Canvas(
root,
width=600,
height=600,
bg="white"
)
canvas.pack(
side="left",
padx=20,
pady=20
)
right_panel = tk.Frame(root)
right_panel.pack(
side="left",
padx=20,
pady=20,
anchor="n"
)
tk.Label(
right_panel,
text="Выберите РМК:",
font=("Arial", 14)
).pack(
anchor="w",
pady=(20, 5)
)
selected_robot = tk.StringVar(value="РМК-2")
robot_menu = tk.OptionMenu(
right_panel,
selected_robot,
"РМК-1",
"РМК-2"
)
robot_menu.config(
font=("Arial", 14),
width=10
)
robot_menu.pack(
anchor="w",
pady=5
)
position_label = tk.Label(
right_panel,
text="Ожидание одометрии...",
font=("Arial", 14),
justify="left"
)
position_label.pack(
anchor="w",
pady=30
)
battery_label = tk.Label(
right_panel,
text="Батарея: нет данных",
font=("Arial", 14)
)
battery_label.pack(
anchor="w",
pady=10
)Важный момент здесь — совместная работа двух циклов событий.
ROS проверяется каждые 10 мс:
root.after(10, ros_spin)а графика обновляется раз в 100 мс:
root.after(100, update_gui)Благодаря этому для простой диагностической программы не требуется создавать отдельный поток для ROS.
5. Карта, преобразование координат и запуск программы
Последняя часть создаёт поле размером 5×5 клеток.
Каждая клетка получает номер от 0 до 24. После этого определяется функция преобразования мировых координат робота в пиксельные координаты Canvas.
Также создаются два графических объекта: круг робота и стрелка его направления. В самом конце запускаются обработка ROS, обновление интерфейса и основной цикл Tkinter.
for y in range(GRID_SIZE):
for x in range(GRID_SIZE):
x1 = MAP_OFFSET + x * CELL_SIZE
y1 = MAP_OFFSET + (GRID_SIZE - 1 - y) * CELL_SIZE
x2 = x1 + CELL_SIZE
y2 = y1 + CELL_SIZE
canvas.create_rectangle(x1,y1,x2,y2,outline="gray")
cell = y * GRID_SIZE + x
canvas.create_text(
x1 + 15,
y1 + 15,
text=str(cell),
fill="gray"
)
def world_to_canvas(x, y):
canvas_x = MAP_OFFSET + CELL_SIZE / 2 + x * CELL_SIZE
canvas_y = MAP_OFFSET + CELL_SIZE * 4.5 - y * CELL_SIZE
return canvas_x, canvas_y
robot_body = canvas.create_oval(0,0,0,0,fill="blue")
robot_arrow = canvas.create_line(0,0,0,0,width=4,fill="blue",arrow=tk.LAST)
root.protocol("WM_DELETE_WINDOW", close_program)
ros_spin()
update_gui()
root.mainloop()Здесь есть одна особенность: система координат ROS и система координат Tkinter направлены по-разному. В ROS положительное направление Y на нашей карте идёт вверх, а у Canvas координата Y увеличивается вниз. Поэтому при преобразовании координат используется:
canvas_y = MAP_OFFSET + CELL_SIZE * 4.5 - y * CELL_SIZEВ результате координаты робота, полученные из одометрии, можно непосредственно показать на нашей сетке 5×5.
При этом если мы хотим вводить координаты вручную, то:
import tkinter as tk
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float32
GRID_SIZE = 5
CELL_SIZE = 100
MAP_OFFSET = 50
def input_cell(name):
while True:
try:
cell = int(input(f"Стартовая клетка {name} (0-24): "))
if 0 <= cell <= 24:
return cell
except ValueError:
pass
print("Введите число от 0 до 24")
def input_direction(name):
while True:
direction = input(
f"Ориентация {name} (W/A/S/D): "
).strip().lower()
if direction in ("w", "a", "s", "d"):
return direction
print("Введите W, A, S или D")
rmc1_cell = input_cell("РМК-1")
rmc1_direction = input_direction("РМК-1")
rmc2_cell = input_cell("РМК-2")
rmc2_direction = input_direction("РМК-2")
robots = {
"РМК-1": {
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"battery": None,
"scan": None,
"start_cell": rmc1_cell,
"direction": rmc1_direction
},
"РМК-2": {
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"battery": None,
"scan": None,
"start_cell": rmc2_cell,
"direction": rmc2_direction
}
}
rclpy.init()
node = Node("fms_visualizer")
def quaternion_to_yaw(q):
sin_yaw = 2.0 * (q.w * q.z + q.x * q.y)
cos_yaw = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
return math.atan2(sin_yaw, cos_yaw)
def update_odometry(name, msg):
robots[name]["x"] = msg.pose.pose.position.x
robots[name]["y"] = msg.pose.pose.position.y
robots[name]["yaw"] = quaternion_to_yaw(
msg.pose.pose.orientation
)
def odom_rmc1(msg):
update_odometry("РМК-1", msg)
def odom_rmc2(msg):
update_odometry("РМК-2", msg)
def battery_rmc1(msg):
robots["РМК-1"]["battery"] = msg.data
def battery_rmc2(msg):
robots["РМК-2"]["battery"] = msg.data
def scan_rmc1(msg):
robots["РМК-1"]["scan"] = msg
def scan_rmc2(msg):
robots["РМК-2"]["scan"] = msg
node.create_subscription(
Odometry,
"/RMC1/odometry",
odom_rmc1,
10
)
node.create_subscription(
LaserScan,
"/RMC1/scan",
scan_rmc1,
qos_profile_sensor_data
)
node.create_subscription(
Float32,
"/RMC1/odrive_voltage",
battery_rmc1,
10
)
node.create_subscription(
Odometry,
"/RMC2/odometry",
odom_rmc2,
10
)
node.create_subscription(
LaserScan,
"/RMC2/scan",
scan_rmc2,
qos_profile_sensor_data
)
node.create_subscription(
Float32,
"/RMC2/odrive_voltage",
battery_rmc2,
10
)
def cell_to_world(cell):
x = cell % GRID_SIZE
y = cell // GRID_SIZE
return x, y
def direction_angle(direction):
if direction == "w":
return 0.0
if direction == "a":
return math.pi / 2
if direction == "s":
return math.pi
if direction == "d":
return -math.pi / 2
def robot_world_position(name):
start_cell = robots[name]["start_cell"]
direction = robots[name]["direction"]
start_x, start_y = cell_to_world(start_cell)
odom_x = robots[name]["x"]
odom_y = robots[name]["y"]
angle = direction_angle(direction)
dx = (
odom_x * math.cos(angle)
- odom_y * math.sin(angle)
)
dy = (
odom_x * math.sin(angle)
+ odom_y * math.cos(angle)
)
world_x = start_x + dx
world_y = start_y + dy
return world_x, world_y
def robot_world_yaw(name):
yaw = robots[name]["yaw"]
direction = robots[name]["direction"]
return yaw + direction_angle(direction)
def world_to_canvas(x, y):
canvas_x = (
MAP_OFFSET
+ CELL_SIZE * 4.5
- y * CELL_SIZE
)
canvas_y = (
MAP_OFFSET
+ CELL_SIZE * 4.5
- x * CELL_SIZE
)
return canvas_x, canvas_y
def draw_robot(name):
x, y = robot_world_position(name)
yaw = robot_world_yaw(name)
canvas_x, canvas_y = world_to_canvas(x, y)
radius = 15
canvas.coords(
robot_body,
canvas_x - radius,
canvas_y - radius,
canvas_x + radius,
canvas_y + radius
)
arrow_length = 35
arrow_x = (
canvas_x
- arrow_length * math.sin(yaw)
)
arrow_y = (
canvas_y
- arrow_length * math.cos(yaw)
)
canvas.coords(
robot_arrow,
canvas_x,
canvas_y,
arrow_x,
arrow_y
)
def draw_lidar(name):
canvas.delete("lidar")
scan = robots[name]["scan"]
if scan is None:
return
robot_x, robot_y = robot_world_position(name)
robot_yaw = robot_world_yaw(name)
for i in range(0, len(scan.ranges), 2):
distance = scan.ranges[i]
if not math.isfinite(distance):
continue
if distance < scan.range_min:
continue
if distance > scan.range_max:
continue
angle = (
scan.angle_min
+ i * scan.angle_increment
)
local_x = distance * math.cos(angle)
local_y = distance * math.sin(angle)
world_x = (
robot_x
+ local_x * math.cos(robot_yaw)
- local_y * math.sin(robot_yaw)
)
world_y = (
robot_y
+ local_x * math.sin(robot_yaw)
+ local_y * math.cos(robot_yaw)
)
canvas_x, canvas_y = world_to_canvas(
world_x,
world_y
)
radius = 2
canvas.create_oval(
canvas_x - radius,
canvas_y - radius,
canvas_x + radius,
canvas_y + radius,
fill="red",
outline="red",
tags="lidar"
)
def update_gui():
name = selected_robot.get()
x = robots[name]["x"]
y = robots[name]["y"]
yaw = robots[name]["yaw"]
start_cell = robots[name]["start_cell"]
direction = robots[name]["direction"].upper()
yaw_degrees = math.degrees(yaw)
position_label.config(
text=(
f"{name}\n\n"
f"Стартовая клетка: {start_cell}\n"
f"Ориентация: {direction}\n\n"
f"Одометрия:\n"
f"x = {x:.2f} м\n"
f"y = {y:.2f} м\n"
f"yaw = {yaw_degrees:.1f}°"
)
)
battery = robots[name]["battery"]
if battery is None:
battery_label.config(
text="Батарея: нет данных"
)
else:
battery_label.config(
text=f"Батарея: {battery:.2f} V"
)
draw_lidar(name)
draw_robot(name)
root.after(100, update_gui)
def ros_spin():
rclpy.spin_once(
node,
timeout_sec=0
)
root.after(
10,
ros_spin
)
def close_program():
node.destroy_node()
rclpy.shutdown()
root.destroy()
root = tk.Tk()
root.title("FMS — визуализация РМК")
root.geometry("900x700")
canvas = tk.Canvas(
root,
width=600,
height=600,
bg="white"
)
canvas.pack(
side="left",
padx=20,
pady=20
)
right_panel = tk.Frame(root)
right_panel.pack(
side="left",
padx=20,
pady=20,
anchor="n"
)
tk.Label(
right_panel,
text="Выберите РМК:",
font=("Arial", 14)
).pack(
anchor="w",
pady=(20, 5)
)
selected_robot = tk.StringVar(
value="РМК-2"
)
robot_menu = tk.OptionMenu(
right_panel,
selected_robot,
"РМК-1",
"РМК-2"
)
robot_menu.config(
font=("Arial", 14),
width=10
)
robot_menu.pack(
anchor="w",
pady=5
)
position_label = tk.Label(
right_panel,
text="Ожидание одометрии...",
font=("Arial", 14),
justify="left"
)
position_label.pack(
anchor="w",
pady=30
)
battery_label = tk.Label(
right_panel,
text="Батарея: нет данных",
font=("Arial", 14)
)
battery_label.pack(
anchor="w",
pady=10
)
for row in range(GRID_SIZE):
for column in range(GRID_SIZE):
x1 = MAP_OFFSET + column * CELL_SIZE
y1 = MAP_OFFSET + row * CELL_SIZE
x2 = x1 + CELL_SIZE
y2 = y1 + CELL_SIZE
canvas.create_rectangle(
x1,
y1,
x2,
y2,
outline="gray"
)
cell = (
(GRID_SIZE - 1 - column) * GRID_SIZE
+ (GRID_SIZE - 1 - row)
)
canvas.create_text(
x1 + 15,
y1 + 15,
text=str(cell),
fill="gray"
)
robot_body = canvas.create_oval(
0,
0,
0,
0,
fill="blue"
)
robot_arrow = canvas.create_line(
0,
0,
0,
0,
width=4,
fill="blue",
arrow=tk.LAST
)
root.protocol(
"WM_DELETE_WINDOW",
close_program
)
ros_spin()
update_gui()
root.mainloop()
Переизобрретение велосипеда? Управлению промышленным манипулятором уже лет тридцать. Всё уже настолько обсосано....
Если разговор идёт о "роботах", то начинать следует именно со зрения. А уже на основе понимания им объектной модели тренировать (именно тренировать) управление манипулятором. К чему все эти "координатные клеточки"?
Зарегистрируйтесь на Хабре, чтобы оставить комментарий
Учим робота собирать клубнику: от управления суставами до движения к ягоде