---
tags: TPW1
---

# TPW1 ROS Server Protocol

[TOC]

## 通訊方式
以ZeroMQ函式庫進行通訊
https://zeromq.org/languages/python/

使用Sub模式接收ROS訊息，位址預設為tcp://ROS ip:7777
使用Push模式發送訊息給ROS，位址預設為tcp://ROS ip:7778

因避免通訊壅塞而採用的第二個接收通道，專門用來傳送地圖資料
使用Sub模式接收ROS訊息，位址預設為tcp://ROS ip:7779

## 通訊格式
使用byte array進行通訊

前7個byte為命令的Header，第7個byte之後的為命令的內容。
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 0 | 1 | 命令編號(ID) |
| 1 | 1 | 傳送序號，ROS收到命令後，會以該序號進行回傳，用以區別回應哪一個命令 |
| 2 | 1 | 錯誤碼 |
| 3-6 | 4 | 命令所附帶資料的長度，以little endian編碼的int |
### Sequence Diagram
#### 一般命令

    sequenceDiagram
    Client->>Car: Command with sn:(ex:1)
    Car->>Client: Ack with sn:(ex:1)

#### 長時間操作命令
* ID: 13 Move to position
* ID: 26 Init patrol
* ID: 27 Deinit patrol

以兩段式回覆
第一階段，確認資料已經送到ros端(由機器人回報ack)
第二階樣，確認是否機器人已經到達目的地(由機器人回報完成)

sequenceDiagram
    participant Client
    participant Car
    Client->>Car: ID:13 Command
    alt ack
        Car->>Client: ID:14 Command
        alt complete
            Car->>Client: ID:15 Command
        else error
            Car->>Client: Command with error code
        end
    else reject error
        Car->>Client: Command with error code
    end

## 命令種類
### (ID: 9) Pos
由ROS端主動發出，以20hz傳出Husky位置
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7-14 | 8 | 磁力車位置 (x)，以little endian編碼的double|
| 15-22 | 8 | 磁力車位置 (y)，以little endian編碼的double |
| 23-30 | 8 | 磁力車位置 (z)，以little endian編碼的double |
| 31-38 | 8 | 磁力車姿態 (roll)，以little endian編碼的double|
| 39-46 | 8 | 磁力車姿態 (pitch)，以little endian編碼的double |
| 47-54 | 8 | 磁力車姿態 (yaw)，以little endian編碼的double |

### (ID: 11) Occupancy Grid
由ROS端主動發出，磁力車傳出SLAM演算法所建立的地圖資訊
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7-14 | 8 | 地圖偏移 (x)，以little endian編碼的double|
| 15-22 | 8 | 地圖偏移 (y)，以little endian編碼的double |
| 23-30 | 8 | 地圖偏移 (z)，以little endian編碼的double |
| 31-34 | 4 | 地圖解析度 (resolution)，以little endian編碼的float |
| 35-38 | 4 | 地圖寬 (w)，以little endian編碼的int |
| 39-42 | 4 | 地圖高 (h)，以little endian編碼的int |
| 43-(43+w*h-1) | w*h | The map data, in row-major order, starting with (0,0). <br> Occupancy probabilities are in the range [0,100].<br> Unknown is -1. |

### (ID: 12) Move Control
由Client端發出，用來遙控Husky
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7-10 | 4 | 以little endian編碼的float，前進與後退方向的值 (-1~1)|
| 11-14 | 4 | 以little endian編碼的float，旋轉方向的值 (-1~1) |

### (ID: 13) Move To Position
由Client端發出，設定移動Husky至固定位置
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7-10 | 4 | 以little endian編碼的float，位置x |
| 11-14 | 4 | 以little endian編碼的float，位置y |
| 15-18 | 4 | 以little endian編碼的float，方向yaw |

此命令分成兩個階段
第一階段，確認資料已經送到ros端(由機器人回報ack: MoveToPositionACK)
第二階樣，確認是否機器人已經到達目的地(由機器人回報: MoveToPositionFinish)

### (ID: 14) Move To Position ACK
ROS收到Client的命令後回傳

### (ID: 15) Move To Position FINISH
移動到固定點之後回傳

### (ID: 16) Move To Position ABORT
由Client端發出，中止移動到固定點的命令

### (ID: 17) PathOccupancy Grid <span style="color:red">調整中，可能改變成僅由Server發出</span>
由Client端發出，請求回傳可行走地圖
- Request command: No data
- Response command:

| 位元 | 長度 | 用途描述 |
| ------------- | ---- | --------------------------------------------------------------------------------------------------------------------------------- |
| 7-14 | 8 | 地圖偏移 (x)，以little endian編碼的double |
| 15-22 | 8 | 地圖偏移 (y)，以little endian編碼的double |
| 23-30 | 8 | 地圖偏移 (z)，以little endian編碼的double |
| 31-34 | 4 | 地圖解析度 (resolution)，以little endian編碼的float |
| 35-38 | 4 | 地圖寬 (w)，以little endian編碼的int |
| 39-42 | 4 | 地圖高 (h)，以little endian編碼的int |
| 43-(43+w*h-1) | w*h | The map data, in row-major order, starting with (0,0). <br> Occupancy probabilities are in the range [0,100].<br> Unknown is -1. |

### (ID: 18) Reset All Map
由Client端發出，重置所有地圖

### (ID: 19) Reset Path Map
由Client端發出，重置可行走路徑地圖

### (ID: 20) Start Record Path
由Client端發出，開始記錄可行走路徑

### (ID: 21) Stop Record Path
由Client端發出，停止記錄可行走路徑

~~### (ID: 22) Go Home
由Client端發出，通知Husky回到充電小屋~~

### (ID: 23) Enable Path Map Stream
由Client端發出，通知Server streaming的內容由原本的Map改為PathMap
- Request command:

| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7 | 1 | 1: enable 0: disable |

- Response command: No data

### (ID: 24) Set Garage point
由Client端發出，以目前所在點，設定為garage點

### (ID: 25) Set Home point
由Client端發出，以目前所在點，設定為鄰近garage的home點

### (ID: 26) Init Patrol
由Client端發出，啟動排程前置作業，目前是從garage前往home點
此命令分成兩個階段
第一階段，確認資料已經送到ros端(由機器人回報ack: InitPatrol)
第二階樣，確認是否機器人已經到達目的地(由機器人回報: MoveToPositionFinish)

### (ID: 27) Deinit Patrol
由Client端發出，結束排程收尾作業，從目前點前往home點，再前往garage點
此命令分成兩個階段
第一階段，確認資料已經送到ros端(由機器人回報ack: DeinitPatrol)
第二階樣，確認是否機器人已經到達目的地(由機器人回報: MoveToPositionFinish)

### (ID: 28) Init Patrol Abort
由Client端發出，中止Init Patrol命令

### (ID: 29) Deinit Patrol Abort
由Client端發出，中止Deinit Patrol命令

### (ID: 40) ROS Status Notification
由ROS端主動發出，頻率為1秒一次，通知Client目前狀態
| 位元 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 7-(7+n-1) | n | 狀態字串，目前狀態有init與online |

### (ID: 99) Keep Alive Signal
由ROS端主動發出，讓Client端確定ROS處於運作狀態，頻率為在無傳送資料時每3秒一次