Удаленное управление роботом Lego Mindstorms по JMX и IP Video

Основной модуль конструктора Lego Mindstorms EV3 может работать с прошивкой leJOS, позволяющей запускать Java-приложения. Специально для этого Oracle выпустил и поддерживает отдельную версию полноценной Java SE.

Нормальная JVM позволила мне использовать встроенный в нее протокол Java Management Extensions (JMX), чтобы реализовать удаленное управление роботом-манипулятором. Для объединения управляющих элементов, показаний датчиков и картинок с установленных на роботе IP-камер используется мнемосхема, сделанная на платформе AggreGate.



Сам робот состоит из двух основных частей: шасси и руки-манипулятора. Они управляются двумя полностью независимыми компьютерами EV3, вся их координация осуществляется через управляющий сервер. Прямого соединения между компьютерами нет.

Оба компьютера подключены к IP-сети помещения через Wi-Fi адаптеры NETGEAR WNA1100. Робот управляется восемью двигателями Mindstorms — из них 4 «большие» и 4 «маленькие». Также установлены инфракрасный и ультразвуковой датчики для автоматической остановки у препятствия при движении задним ходом, два датчика прикосновения для остановки поворота манипулятора из-за препятствия, и гироскопический датчик, облегчающий ориентировку оператора при помощи визуализации положения плеча.

В шасси установлены два двигателя, каждый из которых передает усилие на пару гусеничных приводов. Еще один двигатель поворачивает всю руку-манипулятор целиком на 360 градусов.



В самом манипуляторе два двигателя отвечают за подъем и опускание «плеча» и «предплечья». Еще три двигателя занимаются подъемом/опусканием кисти, ее поворотом на 360 градусов и сжиманием/разжиманием «пальцев».



Самым сложным механическим узлом является «кисть». Из-за необходимости выноса трех тяжелых двигателей в район «локтя» конструкция получилась достаточно хитрой.



В целом все выглядит так (коробок спичек был с трудом найден для масштаба):



Для передачи картинки установлены две камеры:

• Обычный Android-смартфон с установленным приложением IP Webcam для общего обзора (на снимке HTC One)
• Автономная Wi-Fi микро-камера AI-Ball, установленная прямо на «кисти» манипулятора и помогающая хватать предметы сложной формы

Программирование EV3

ПО самого робота получилось максимально простым. Программы двух компьютеров очень похожи, они запускают JMX сервер, регистрируют MBean'ы, соответствующие двигателям и датчикам, и засыпают в ожидании операций по JMX.

Код главных классов ПО руки-манипулятора

public class Arm
{
  
  public static void main(String[] args)
  {
    try
    {
      EV3Helper.printOnLCD("Starting...");
      
      EV3Helper.startJMXServer("192.168.1.8", 9000);
      
      MBeanServer mbs = ManagementFactory.getPlatformMBeanServer();
      
      EV3LargeRegulatedMotor motor = new EV3LargeRegulatedMotor(BrickFinder.getDefault().getPort("A"));
      LargeMotorMXBean m = new LargeMotorController(motor);
      ObjectName n = new ObjectName("robot:name=MotorA");
      mbs.registerMBean(m, n);
 
      // Registering other motors here
     
      EV3TouchSensor touchSensor = new EV3TouchSensor(SensorPort.S1);
      TouchSensorMXBean tos = new TouchSensorController(touchSensor);
      n = new ObjectName("robot:name=Sensor1");
      mbs.registerMBean(tos, n);
      
      // Registering other sensors here
      
      EV3Helper.printOnLCD("Running");
      
      Sound.beepSequenceUp();
      
      Thread.sleep(Integer.MAX_VALUE);
    }
    catch (Throwable e)
    {
      e.printStackTrace();
    }
  }
  
}

public class EV3Helper
{
  
  static void startJMXServer(String address, int port)
  {
    MBeanServer server = ManagementFactory.getPlatformMBeanServer();
    
    try
    {
      java.rmi.registry.LocateRegistry.createRegistry(port);
      
      JMXServiceURL url = new JMXServiceURL("service:jmx:rmi:///jndi/rmi://" + address + ":" + String.valueOf(port) + "/server");
      
      Map<String, Object> props = new HashMap<String, Object>();
      props.put("com.sun.management.jmxremote.authenticate", "false");
      props.put("com.sun.management.jmxremote.ssl", "false");
      
      JMXConnectorServer connectorServer = JMXConnectorServerFactory.newJMXConnectorServer(url, props, server);
      connectorServer.start();
    }
    catch (Exception e)
    {
      e.printStackTrace();
    }
  }
  
  static void printOnLCD(String s)
  {
    LCD.clear();
    LCD.drawString(s, 0, 4);
  }
  
}

Для каждого типа датчика и мотора создан интерфейс MBean'а и реализующий его класс, которые напрямую делегирует все вызовы классу, входящему в leJOS API.

Пример кода интерфейса

public interface LargeMotorMXBean
{
  public abstract void forward();
  
  public abstract boolean suspendRegulation();
  
  public abstract int getTachoCount();
  
  public abstract float getPosition();
  
  public abstract void flt();
  
  public abstract void flt(boolean immediateReturn);
  
  public abstract void stop(boolean immediateReturn);
  
  public abstract boolean isMoving();
  
  public abstract void waitComplete();
  
  public abstract void rotateTo(int limitAngle, boolean immediateReturn);
  
  public abstract void setAcceleration(int acceleration);
  
  public abstract int getAcceleration();
  
  public abstract int getLimitAngle();
  
  public abstract void resetTachoCount();
  
  public abstract void rotate(int angle, boolean immediateReturn);
  
  public abstract void rotate(int angle);
  
  public abstract void rotateTo(int limitAngle);
  
  public abstract boolean isStalled();
  
  public abstract void setStallThreshold(int error, int time);
  
  public abstract int getRotationSpeed();
  
  public abstract float getMaxSpeed();
  
  public abstract void backward();
  
  public abstract void stop();
  
  public abstract int getSpeed();
  
  public abstract void setSpeed(int speed);
}

Пример кода реализации MBean'а

public class LargeMotorController implements LargeMotorMXBean
{
  final EV3LargeRegulatedMotor motor;
  
  public LargeMotorController(EV3LargeRegulatedMotor motor)
  {
    this.motor = motor;
  }
  
  @Override
  public void forward()
  {
    motor.forward();
  }
  
  @Override
  public boolean suspendRegulation()
  {
    return motor.suspendRegulation();
  }
  
  @Override
  public int getTachoCount()
  {
    return motor.getTachoCount();
  }
  
  @Override
  public float getPosition()
  {
    return motor.getPosition();
  }
  
  @Override
  public void flt()
  {
    motor.flt();
  }
  
  @Override
  public void flt(boolean immediateReturn)
  {
    motor.flt(immediateReturn);
  }

  // Similar delegating methods skipped
}

Как ни странно, на этом программирование закончилось. На стороне сервера и операторского рабочего места не было написано ни одной строчки кода.

Подключение к серверу

Непосредственное управление роботом осуществляет сервер IoT-платформы AggreGate. Установленная бесплатная версия продукта AggreGate Network Manager включает драйвер протокола JMX и позволяет подключить до десяти JMX-хостов. Нам понадобится подключить два — по одному на каждый кирпичик EV3.

Прежде всего, нужно создать аккаунт JMX устройства, указав в настройках URL, заданный при запуске JMX сервера:

Свойства соединения с JMX-устройством

После этого выбираем активы (т.е. MBean'ы в данном случае), которые будут добавлены в профиль устройства:

Выбор MBean'ов

И через несколько секунд смотрим и меняем текущие значения всех опрошенных свойств MBean'ов:

Снимок устройства

Можно также потестировать различные операции вызывая вручную методы MBean'ов, например forward() и stop().

Список операций

Далее настраиваем периоды опроса для датчиков. Высокая частота опроса (100 раз в секунду) используется, так как управляющий сервер находится в локальной сети вместе с роботом и именно сервер принимает решения об остановке вращения при упоре в препятствие и т.п. Решение, безусловно, не промышленное, но в хорошо работающей Wi-Fi сети в рамках одной квартиры показало себя вполне адекватным.

Периоды опроса

Интерфейс оператора

Теперь переходим к созданию интерфейса оператора. Для этого сначала создаем новый виджет и накидываем в него нужные компоненты. В конечном работающем варианте выглядит он так:



По сути, весь интерфейс состоит из нескольких панелей с кнопками, слайдерами и индикаторами, сгруппированными в различные сеточные раскладки, и двух больших видео-плееров, транслирующих картинки с камер.

Вид изнутри редактора интерфейсов

Вся форма:


Вид с показанными панелями-контейнерами:

Теперь, как говорят АСУТПшники, осталось «оживить мнемосхему». Для этого применяются так называемые привязки связывающие свойства и методы графических компонентов интерфейса со свойствами и методами серверных объектов. Так как компьютеры EV3 уже подключены к серверу, серверными объектами могут быть и MBean'ы нашего робота.

Весь интерфейс оператора содержит около 120 привязок, большая часть из которых однотипна:



Половина однотипных привязок реализует управление при помощи кликов на кнопки, расположенные на мнемосхеме. Это красиво, удобно для тестирования, но совершенно непригодно для реального передвижения робота и перемещения грузов. Активаторами привязок из этой группы являются события mousePressed и mouseReleased различных кнопок.

Вторая половина привязок позволяет управлять роботом с клавиатуры, предварительно нажав на кнопку Keyboard Control. Эти привязки реагируют на события keyPressed и keyReleased, а в условии каждой привязки прописано, на какой именно код кнопки нужно реагировать.

Все управляющие привязки вызывают методы forward(), backward() и stop() различных MBean'ов. Поскольку доставка событий происходит асинхронно, важно, чтобы вызовы функций forward()/backward() и последующие вызовы stop() не перепутались. Для этого привязки, вызывающие методы одного MBean'а, добавлены в одну очередь (Queue).

Две отдельные группы привязок выставляют начальные скорости и ускорения двигателей (сейчас это реализовано на стороне сервера при помощи модели, поэтому эти привязки отключены) и меняют скорости/ускорения при перемещении ползунков Speed и Acceleration.

Привязки скорости и ускорения двигателей

Все остальные привязки подсвечивают части мнемосхемы при активации сенсоров касания, показывают измеренные сенсорами расстояния до объектов на шкалах и выполняют различные служебные функции.

Остальные привязки

Помимо аккаунтов JMX-устройств и вышеописанного виджета на сервере есть еще один объект, участвующий в управлении роботом. Это модель, которая останавливает двигатели при срабатывании одного из датчиков прикосновения. Осуществляется это также при помощи привязок, различие их с привязками виджета лишь в том, что привязки модели обрабатываются на сервере и связывают одни свойства серверных объектов с другими.

Привязки модели управления

Закончив настройку виджета и модели можно запускать виджет, активировать клавиатурное управление и развлекаться: