# motion_planning.srv

###### tags: `gipick_info`, `srv`

## Description

主要提供 路徑規劃 (motion_planning) 的相關功能, 以及與手臂運動學相關功能, 如果要使用路徑規劃功能, 典型用法如下： 先使用其中一種規劃指令 `command` = `JOINT_PLAN` or `PRE_PLAN` or `MULTI_PLAN`, 帶入相對應的 input 參數, 此時會開始規劃路徑 (同步的, 規劃完成後才會 return), 若收到的 `error_code` = `ERR_SUCCESS` 代表規劃成功。 接著再透過指令 `command` = `GET_TRJECTORY`, 會得到最新的 路徑資訊 [`trajectory_msgs/JointTrajectory[]`](http://docs.ros.org/melodic/api/trajectory_msgs/html/msg/JointTrajectory.html)。

## Syntax

// Command area ////////////////////
uint8 JOINT_PLAN=0
uint8 PRE_PLAN=1
uint8 MULTI_PLAN=2
uint8 GET_TRAJECTORY=3
uint8 SUB_SAMPLE_TRAJECTORY=4
uint8 GET_FK_POSE=5

uint8 command
////////////////////////////////////

// Input value area ////////////////
string frame
string link
geometry_msgs/Pose[] poses
float64[] joints
////////////////////////////////////

---

// Error code area /////////////////
int8 ERR_SUCCESS = 0
int8 ERR_IK_FAIL = 1
int8 ERR_PRE_PLAN_FAIL = 2
int8 ERR_JOINT_PLAN_FAIL = 3
int8 ERR_MULTI_PLAN_FAIL = 4
int8 ERR_ROTATE_OVER_LIMIT = 5

int32 error_code
////////////////////////////////////

// Output value area ///////////////
geometry_msgs/Pose pose
trajectory_msgs/JointTrajectory[] trajectory_paths
int32 current_pose_index
////////////////////////////////////

### Command: JOINT_PLAN=0

planner 會將`joints` 此六軸角度視為 target joints 去做規劃路徑 (同步的), 規劃起始位置會在 current_state(reported by the robot's state publication)

#### Input

|型態 | 變數名 | 描述 |
| -------- | -------- | :-- |
| float64[]| `joints` | target joints 六軸角度(radian) |

#### Output

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| int32| `error_code` | 若 `error_code` = `ERR_SUCCESS` 表示規劃成功, `error_code` = `ERR_JOINT_PLAN_FAIL` 表示規劃失敗|

### Command: PRE_PLAN=1

planner 會從 規劃起始位置`joints` 開始規劃, 將 target (`link`) 沿著 target waypoints (`poses`) 規劃, 過程中若有 waypoint 規劃失敗就會直接回傳 `error_code` , 不會全部規劃完

整個規劃過程是同步的

#### Input

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| string |`frame` | 帶入欲規劃 `poses` 所在的 `frame` |
| string |`link` | target `link`
| float64[] | `joints` | 規劃起始位置六軸 |
|[geometry_msgs/Pose[]](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Pose.html)| `poses`| target waypoints `poses`|

#### Output

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| int32| `error_code` | 若 `error_code` = `ERR_SUCCESS` 表示規劃成功, `error_code` = `ERR_IK_FAIL` 表示在計算 Ik 的階段就失敗了, 也表示這個 pose 手臂到不了, `error_code` = `ERR_PRE_PLAN_FAIL` 表示規劃失敗, 有可能是碰撞, 也可能是沒找到解|

### Command: MULTI_PLAN=2

planner 會從 規劃起始位置`joints` 開始規劃,將 target `link` 規劃到 targets `poses` 任一個 pose 上, 任何一個 pose 都有可能規劃成功 (機率最高的那個 pose), 也有可能全部都規劃失敗(找不到解)

整個規劃過程是同步的

為了讓使用者知道規劃成功的是哪個 pose, `current_pose_index` 會回傳規劃成功的 `poses` index

#### Input

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| string |`frame` |帶入欲規劃 target `pose` 所在的 `frame` |
| string |`link` | target `link` |
| float64[] | `joints` | 規劃起始位置六軸 |
|[geometry_msgs/Pose[]](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Pose.html)| `poses`| targets `poses`|

#### Output

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| int32 | `error_code` | 若 `error_code` = `ERR_SUCCESS` 表示規劃成功, `error_code` = `ERR_MULTI_PLAN_FAIL` 表示規劃失敗, 有可能是碰撞, 也可能是沒找到解|
| int32 | `current_pose_index` | 會回傳規劃成功的 `poses` index |

### Command: GET_TRAJECTORY=3

從 planner 中取回成功規劃的規劃路徑 (trajectory)

#### Input

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| | | |

#### Output

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| int32| `error_code` | (尚未完成)|
|[trajectory_msgs/JointTrajectory[]](http://docs.ros.org/melodic/api/trajectory_msgs/html/msg/JointTrajectory.html)| `trajectory_paths` | 成功規劃的規劃路徑 |

### Command: SUB_SAMPLE_TRAJECTORY=4

// 目前沒有用到

### Command: GET_FK_POSE=5

給 planner `link` 名稱, 以及 六軸角度 `joints`, 可以算出 `link` 所在的 pose

#### Input

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| string | `link` | target `link` |
| float64[] | `joints` | 目前位置六軸 |

#### Output

|型態 | 變數名 | 描述 |
| -------- | -------- | -- |
| int32| `error_code` | (尚未撰寫) |
| [geometry_msgs/Pose](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Pose.html) | `pose`| `link` 所在的 pose |