Комментарии (6)

1. 

   Sencis

   16.09.2026 07:05

   #30432256

   Вот скажите Владимир, этот код который вы выложили, вы хотя-бы в симуляторе проверили? Или на вашем самодельном манипуляторе? Если да, почему не добавить в статью gif анимации/видео как оно работает. Если нет и это просто сгенерированный LLM код, оторванный от реальности не выкладывайте его, LLM сочинят всякий бред который зачастую выглядит очень правдоподобно.

   
   

   1. 

      wowa144 Автор

      16.09.2026 07:05

      #30432300

      Добрый день!
      Тестировали в симуляторе, работает. Немного позже приложу доказательство. На самом деле код очень простой и это скорее начало пути для управления манипулятором, в будущих статьях покажем работу и в симуляторе, и на реальном прототипе в теплице. Решение разработано несколько месяцев назад, только сейчас руки доходят начинать описывать.

      
      

      1. 

         Sencis

         16.09.2026 07:05

         #30433810

         Хорошо, хотелось-бы видеть больше тестов на практике, особенно на настоящем манипуляторе.

1. 

   wowa144 Автор

   16.09.2026 07:05

   #30435192

   

   В качестве дополнения можно создать простой интерфейс, используя какой-нибудь фреймворк интерфейса и 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.

   
   

   1. 

      wowa144 Автор

      16.09.2026 07:05

      #30435332

      При этом если мы хотим вводить координаты вручную, то:

      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()

      
      

      1. 

         Sencis

         16.09.2026 07:05

         #30436366

         Ок, я python не знаю( и никогда на нем ничего не кодил, только на C/C++. Робот похоже на самодельных серводвижках с as5600 не на китайских сервах, что хорошо значит по координатам будет работать плавно если грамотно всё сделать.