---
tags: TPW1
---
[toc]
# TPW1 Streaming Protocol

## RGB camera
rtsp://admin:admin@[Camera ip]:7070/

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

使用Sub模式接收訊息，位址預設為tcp://[WIN_PC Server ip]:17781

### 通訊格式
使用byte array進行通訊，如遇到byte轉位元，皆以little endian編碼

| 陣列位置 | 長度 | 用途描述 |
| --- | -------- | -------- |
| 0 | 2 | 圖片width : 2bytes short |
| 2 | 2 | 圖片height : 2bytes short |
| 4 | 1 | 圖片pixel位元數 (暫時只支援32bits) |
| 5 | 4 | 圖片資料長度，會相等於width * height * 4 |
| 9 | 圖片資料長度 | 圖片內容，每個pixel 4bytes，可以<a href="#附注1">附注1</a>算式還原
| 9+圖片資料長度 | 1 | 測量點資料長度 |
| 9+圖片資料長度+1 | 測量點資料長度 | 每點長度為12bytes<br>x : 2bytes short<br>y : 2bytes short<br> temp : 8bytes double
| 9+圖片資料長度+1+測量點資料長度 | 1 | 測量框資料長度 |
| 9+圖片資料長度+1+測量點資料長度+1| 測量框資料長度 | 每點長度為16bytes<br>x : 2bytes short<br>y : 2bytes short<br>width: 2bytes short<br>height : 2byte short<br>temp : 8bytes double

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

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

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

### 命令種類
#### (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. |

## 附注1

int value = buf[0] & 0xff 
            | (buf[1] & 0xff ) << 8
            | (buf[2] & 0xff ) << 16
            | (buf[3] & 0xff ) << 24