Pull to refresh

ROS, ELM и черепашка

Reading time 6 min
Views 5.9K
Robotic Operation System позволяет взаимодействовать своим подсистемам по механизмам «подписка на топик» и «вызов сервиса» по своему специальному протоколу. Но есть пакет rosbridge, который позволяет общаться с ROS извне с помощью websocket. Описанный протокол позволяет выполнять основные операции по взаимодействию с другими подсистемами.

ELM — очень простой и элегантный язык, компилирующийся в javascript и отлично подходящий для разработки интерактивных программ.

Я решил совместить приятное с полезным и изучать ROS (по которой сейчас идет курс) и ELM вместе.

В ROS есть демонстрационный модуль turtlesim, эмулирующий робота-черепашку. Один из предоставляемых им узлов рисует движение черепашки в своем окне, другой — преобразует нажатия стрелок на клавиатуре в команды движения и поворотов черепашки. К этому процессу можно подключиться из простой программы на ELM.

ELM использует паттерн model-updater-view. Состояние программы описывается типом данных Model, функция update берет входящие события типа Msg и преобразует старую модель в новую (и, возможно, операцию, которую надо выполнить), а функция view по модели строит ее представление в пользовательком интерфейсе, который может порождать события типа Msg. Еще события могут приходить по подпискам, которые создаются специальной функцией из модели.

Обобщенная web-программа на ELM выглядит так:

init : ( Model, Cmd Msg )
update : Msg -> Model -> ( Model, Cmd Msg )
view : Model -> Html Msg
subscriptions : Model -> Sub Msg
main =
  Html.program
    { init = init
    , view = view
    , update = update
    , subscriptions = subscriptions
    }

а программисту остается только реализовать эти четыре функции.

Опишем модель:

type alias Model =
  { x : Float
  , y : Float                      -- координаты черепашки
  , dir : Float                    -- направление, в котором черепашка смотрит

  , connected : Bool          -- подключенность к серверу
  , ws : String                  -- URL websocket, который слушает rosbridge
                                   -- если ROS запущен на рабочей машине
                                   -- и все настроено поумолчанию,
                                   -- url будет ws://localhost:9090/
  , topic : String               -- топик, по которому управляется черепашка,
                                   -- обычно /turtle1/cmd_vel

  , input : String              -- JSON сообщение, которое мы можем редактировать
                                  -- и отправить в систему руками
  , messages : List String  -- Пришедшие со стороны rosbridge сообщения
                                   -- эти поля требуются только для отладки
                                   -- и в исследовательских целях
  }

init : ( Model, Cmd Msg )
init =
  ( Model 50 50 0 False "ws://192.168.56.101:9090/" "/turtle1/cmd_vel" "" []
  , Cmd.none
  )

Пока ни чего сложного, модель представляет из себя структуру с именованными полями.
Тип Msg устроен менее привычно для ОО-программистов:

type Msg
  = Send String
  | NewMessage String
  | EnterUrl String
  | EnterTopic String
  | Connect
  | Input String

Это так называемый алгебраический тип, описывающий прямую (размеченную) сумму нескольких альтернатив. Наиболее близкое предстваление этого типа в ООП — Msg объявляется абстрактным классом, а каждая строка алитернативы описывает новый, унаследованный от Msg, конкретный класс. Input, Send и прочее — это имена-конструкторы этих классов, за которыми следуют параметры конструктора, которые превращаются в поля класса.

Каждая альтернатива это запрос на изменение модели и выполнение каких-либо операций, который порождается действиями пользователя с интерфейсом (view) или внешними событиями — получением данных из websocket.

  • Send String — запрос на отправку строки в websocket
  • NewMessage String — обработать принятую из websocket строку
  • EnterUrl String — редактируется url для websocket
  • EnterTopic String — редактируется топик
  • Connect — закончить редактирование настроек и связаться с сервером
  • Input String — редактирование «ручного» сообщения в websocket

Теперь более-менее понятно, как реализовать функцию update:

update : Msg -> Model -> ( Model, Cmd Msg )
update msg model =
  case msg of
    EnterTopic newInput
     -> ( { model | topic = newInput }, Cmd.none )
    EnterUrl newInput
     -> ( { model | ws = newInput }, Cmd.none )
    Connect
     -> ( { model | connected = True }, WebSocket.send model.ws (subscr model.topic) )
    Input newInput
     -> ( { model | input = newInput }, Cmd.none )
    Send data
     -> ( { model | input = "" }, WebSocket.send model.ws data )
    NewMessage str
     -> case Decode.decodeString (decodePublish decodeTwist) str of
          Err _
           -> ( { model | messages = str :: model.messages }, Cmd.none )
          Ok t
           -> let ( r, a ) = turtleMove t.msg
                  dir = model.dir + a
              in  ( { model
                    | x = model.x + r * sin dir
                    , y = model.y + r * cos dir
                    , dir = dir
                    , messages = str :: model.messages
                    }
                  , Cmd.none
                  )

Здесь используются несколько функций, которые мы определим позднее:

  • subscr: String -> String — конструирует строку запроса для подписки на топик в rosbridge
  • (decodePublish decodeTwist) — декодирование сообщения от топика, содержащее данные ROS-типа geometry_msgs/Twist, с которыми оперирует черепашка
  • turtleMove: Twist -> ( Float, Float ) — извлечение из сообщения перемещения и угла поворота черепашки

А пока определим функцию view:

view : Model -> Html Msg
view model =
  div [] <|
    if model.connected
    then let x = toString model.x
             y = toString model.y
             dirx = toString (model.x + 5 * sin model.dir)
             diry = toString (model.y + 5 * cos model.dir)
         in  [ svg [ viewBox "0 0 100 100", Svg.Attributes.width "300px" ]
                 [ circle [ cx x, cy y, r "4" ] []
                 , line [ x1 x, y1 y, x2 dirx, y2 diry, stroke "red" ] []
                 ]
             , br [] []
             , button [ onClick <| Send <| pub model.topic 0 1 ]
                 [ Html.text "Left" ]
             , button [ onClick <| Send <| pub model.topic 1 0 ]
                 [ Html.text "Forward" ]
             , button [ onClick <| Send <| pub model.topic -1 0 ]
                 [ Html.text "Back" ]
             , button [ onClick <| Send <| pub model.topic 0 -1 ]
                 [ Html.text "Rigth" ]
             , br [] []
             , input [ Html.Attributes.type_ "textaria", onInput Input ] []
             , button [ onClick (Send model.input) ] [ Html.text "Send" ]
             , div [] (List.map viewMessage model.messages)
             ]
    else [ Html.text "WS: "
         , input
             [ Html.Attributes.type_ "text"
             , Html.Attributes.value model.ws
             , onInput EnterUrl
             ]
             []
         , Html.text "Turtlr topic: "
         , input
             [ Html.Attributes.type_ "text"
             , Html.Attributes.value model.topic
             , onInput EnterTopic
             ]
             []
         , br [] []
         , button [ onClick Connect ] [ Html.text "Connect" ]
         ]

viewMessage : String -> Html msg
viewMessage msg = div [] [ Html.text msg ]

view создает DOM (можно чтитать, что просто html). Каждый объект (тег) генерируется отдельной функцией из библиотеки «elm-lang/html», которая принимает два параметра — список аттрибутов, типа Html.Attribute и список вложенных объектов/тегов. (Лично я считаю такое решение неудачным — я как-то поместил вложенный элемент в тег br и потом долго не мог найти его на экране, правильная библиотека не должна позволить сделать такую ошибку, оставив у br только аргумент с аттрибутами. Но возможно, в таком подходе есть глубокий смысл для специалистов во фронтетде.)

Отдельно я хочу описать аттрибуты. Тип Html.Attribute — это сборная-солянка для совершенно разнородных сущностей. Например Html.Attributes.type_ : String -> Html.Attribute msg задает тип в таких тегах, как imput, а Html.Events.onClick : msg -> Html.Attribute msg задает событие, которое должно произойти при клике на этот элемент.

Полностью прописать Html.Attributes.type_ в коде пришлось из за конфликта с Svg.Attributes.type_.

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

onClick <| Send <| pub model.topic 0 1

Он эквивалентен

onClick (Send (pub model.topic 0 1))

<| — это оператор применения функции к аргументу (в Haskell он называется '$'), который позволяет использовать меньше скобок.

onClick — уже рассмотренная создания аттрибута, ее параметр — генерируемое событие.

Send — один их конструкторов типа Msg, ее патаметр — строка, которую мы хотим потом отправить в websocket.

Конструкторы и типы в ELM пишутся с большой буквы, а переменные (точнее константы и параметры функций), обычные и типовые, с маленькой.

pub model.topic 0 1 — вызов функции создания запроса на отправку сообщения о движении черепашки на топик. Топик берется из модели, а 0 и 1 — перемещение и поворот.

Опишем недостающие функции. Проще всего создавать сообщения для отправки в websocket, так как это просто строки:

subscr : String -> String
subscr topic = "{\"op\":\"subscribe\",\"topic\":\"" ++ topic ++ "\"}"

pub : String -> Float -> Float -> String
pub topic m r =
  "{\"topic\":\""
    ++ topic
    ++ "\",\"msg\":{\"linear\":{\"y\":0.0,\"x\":"
    ++ toString m
    ++ ",\"z\": 0.0},\"angular\":{\"y\":0.0,\"x\":0.0,\"z\":"
    ++ toString r
    ++ "}},\"op\":\"publish\"}"

С обработкой сообщений немного сложнее. Тип сообщения, с которым работает turtlesim можно посмотреть средствами ROS:

ros:~$ rosmsg info geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

rosbridge его превращает в json и заворачивает в сообщение о событии на топике.

Декодирование его будет выглядеть так:

type alias Vector3 = ( Float, Float, Float )

type alias Twist = { linear : Vector3, angular : Vector3 }

decodV3 : Decode.Decoder Vector3
decodV3 =
  Decode.map3 (,,)
    (Decode.at [ "x" ] Decode.float)
    (Decode.at [ "y" ] Decode.float)
    (Decode.at [ "z" ] Decode.float)

decodeTwist : Decode.Decoder Twist
decodeTwist =
  Decode.map2 Twist
    (Decode.at [ "linear" ] decodV3)
    (Decode.at [ "angular" ] decodV3)

type alias Publish a = { msg : a, topic : String, op : String }

decodePublish : Decode.Decoder a -> Decode.Decoder (Publish a)
decodePublish decMsg =
  Decode.map3 (\t m o -> { msg = m, topic = t, op = o })
    (Decode.at [ "topic" ] Decode.string)
    (Decode.at [ "msg" ] decMsg)
    (Decode.at [ "op" ] Decode.string)

Декодер Json-представления некоторого типа комбинируется из других декодеров.
Decode.map3 (,,) применяет три декодера, переданные ему в параметрах, и создает тупл из трех декодорованных элементов с помощью операции (,,).

Decode.at декодирует величину, извлеченную по данному пути в Json заданным декодером.

Код

(\t m o -> { msg = m, topic = t, op = o })

описывает замыкание. Он аналогичен коду на js:

function (t,m,o) { return {"msg":m, "t":t, "op":p} }

Полный код можно взять с github.

Если есть желание попробовать ROS придется установить самостоятельно. Вместо установки ELM можно воспользоваться сервисом.
Tags:
Hubs:
+12
Comments 5
Comments Comments 5

Articles