[FEAT]: System tests

[Iranaphor]

Description

Building on #238 a key system test should be the testing of gotonode action server and executepolicymode action server where a conceptual robot is positioned at various percentages along an edge (0%:0.05%:1%) with each instance evaluated for successful action server execution.

Testing conducted under GTN action server should include robot positioned along edge C_D on a graph of A-B-C-D-E-F, the testing should then be the robot targeting each of the 6 nodes, verifying planing backwards and forwards from each intermediate pose on C_D.

Testing conducted under EMP action server should include robot positioned along edge C_D on a graph of A-B-C-D-E-F:

1. route is from A-F (robot half way)
2. route is from C-F (robot is close to start)
3. route is from A-D (robot is close to end)
4. route is from C-D (robot is close to start and end)

Additional Information

No response