[I] Custom Path Planning Configuration

The path planning module uses two configuration files to create a graph of poses that is used for trajectory planning:

• poses.yaml contais all poses that are used for planning and
• neighbors.yaml defines neighborhood relations between those poses.

###Example

The easiest way to explain the configuration files is with a small example. Consider a graph with five poses:

These poses can e.g. be two grip poses and two cargo poses and the ARM_UP pose in between. It is only safe for the arm to move between poses that are connected. A movement from FRONT_LEFT to BACK_LEFT directly might e.g. cause a collision with a wall on the left side. The poses.yaml can be created with the GUI (luh_youbot_gui) and looks something like this:

---
CENTER:
  - arm_joint_1: 202
  - arm_joint_2: 65
  - arm_joint_3: -146
  - arm_joint_4: 102.5
  - arm_joint_5: 172
---
BACK_LEFT:
  - arm_joint_1: 41.8
  - arm_joint_2: 74.3
  - arm_joint_3: -56.0
  - arm_joint_4: 181.3
  - arm_joint_5: 169
---
BACK_RIGHT:
  - arm_joint_1: 2.49
  - arm_joint_2: 74.3
  - arm_joint_3: -56.0
  - arm_joint_4: 181.3
  - arm_joint_5: 169
---
FRONT_LEFT:
  - arm_joint_1: 172
  - arm_joint_2: 99.3
  - arm_joint_3: -64.4
  - arm_joint_4: 169.6
  - arm_joint_5: 171.3
---
FRONT_RIGHT:
  - arm_joint_1: 232
  - arm_joint_2: 99.3
  - arm_joint_3: -64.4
  - arm_joint_4: 169.6
  - arm_joint_5: 171.3

The neighbors.yaml defines neighborhood pairs and looks like this:

- CENTER: FRONT_LEFT
- CENTER: FRONT_RIGHT
- CENTER: BACK_LEFT
- CENTER: BACK_RIGHT
- FRONT_LEFT: FRONT_RIGHT
- BACK_LEFT: BACK_RIGHT

Each line is one neighborhood pair. It is important that every pose listed in poses.yaml has at least one neighbor and that all poses are connected to one graph. The files can be saved in the package that uses the poses e.g. in your_package/poses/.

####Different Pose Files

The pose file described here does not have to be the same as the one in the previous tutorial. The pose file from the previous tutorial is just used to look up predefined poses. The pose file described here is used for trajectory planning. However in most cases it is probably a good idea to use the same file for both.

####Trajectory Planning

If start and goal pose are given, the planner finds the closest two poses in the graph and plans a trajectory between them. In the example below, the closest pose to start is FRONT_LEFT and the closest to goal is BACK_RIGHT. In the first step a trajectory from FRONT_LEFT to BACK_RIGHT is calculated: FRONT_LEFT → CENTER → BACK_RIGHT. For the final trajectory the start and goal poses are substituted which leads to: START → CENTER → GOAL.

####Including Custom Files

To include your custom files you just have to set the parameters to your file locations like in the previous tutorial:

<launch>
	<arg name="youBotHasBase" default="true"/>
	<arg name="youBotHasArm" default="true"/>
 
        <!-- LOAD DEFAULT DRIVER PARAMETERS -->
        <include file="$(find luh_youbot_driver_api)/launch/load_default_parameters.launch">
                <arg name="youBotHasArm" value="$(arg youBotHasArm)"/>
                <arg name="youBotHasBase" value="$(arg youBotHasBase)"/>
        </include>
 
	<!-- LOAD DEFAULT CONTROLLER PARAMETERS -->
	<include file="$(find luh_youbot_controller)/launch/load_default_parameters.launch" />
 
        <!-- DEFINE FILE LOCATIONS -->
        <param name="/module_motion_planner/poses_file" value="$(find your_package)/poses/poses.yaml" />
        <param name="/module_motion_planner/neighbors_file" value="$(find your_package)/poses/neighbors.yaml" />
 
        <!-- LAUNCH STATE PUBLISHER -->
        <include file="$(find luh_youbot_controller)/launch/youbot_state_publisher.launch">
                <arg name="youBotHasArm" value="$(arg youBotHasArm)"/>
                <arg name="youBotHasBase" value="$(arg youBotHasBase)"/>
        </include>
 
       	<!-- LAUNCH CONTROLLER NODE -->
	<node name="controller_node" pkg="luh_youbot_controller" type="controller_node" output="screen" />
 
</launch>