The path planner produces a collision-free path leading the robot towards the goal. However, it is a sequence of waypoints that does not explicitly says how the path should be followed. To navigate the robot through the environment, the path has to be transformed in the sequence of actuators' commands leading to following the path towards the goal. In the case of the robot used within this assignment, it means a sequence of commands consisting of forward linear velocity and the robot's angular velocity. The most straightforward approach to path following is the “turn and move” strategy that consists of a sequence of turns with a zero forward velocity and a sequence of forward moves with a zero angular velocity. However, due to frequent stops, this approach results in a very slow path following.

The more effective approach, which is often used for differential wheel robots, is a “pure pursuit law” that is often called the “carrot following” approach. For simplicity, we will assume that the forward linear velocity of the robot is constant for the whole path, and we are searching only for the angular velocity commands. In this case, the pure pursuit law consists of two steps:

1. Find a position $P_d$ on the path in a look-ahead distance $d_l$ from the robot's current position.
2. Compute desired angular velocity $\omega$ that given the constant forward velocity $v$ results in reaching position $P_d$.

Considering a constant control inputs, curve $k$ that leads the robot to desired position $P_d$ is part of the circle with diameter $R$. From the presented scheme, we can derive that $R$ can be computed based on equation $$ R = \frac{{d_l}^2}{2\Delta y}. $$ With a given constant velocity $v$ and computed radius $R$, the desired angular velocity $\omega$ that will result in a following of curve $k$ is given by $$ \omega = \frac{v} {R}. $$

The complete path-following approach based on the pure pursuit control law is implemented by repeating the following steps in a control loop running at a certain frequency (usually ranging from one Hz to tens of Hz, depending on the dynamics of a robot):

1. Identify the lookahead point,
2. compute the desired control inputs (forward and angular velocity),
3. apply the control inputs.

Such an approach enables compensation for the inaccuracies of both the localization algorithm and actuators.

Another approach for path following (again assuming constant forward velocity) is applying PID (P, PI, PD) controller for orientation/heading control. The reference heading for this controller is again computed based on the position of the look-ahead point and the current position of the robot. The control error is then given as a deviation from this reference heading, and the control input is computed based on equation

$$ \omega_n = k_p e_n + k_I \sum_{i=1}^n (e_n t_s) + k_d \frac{1}{t_s} (e_n - e_{n-1}), $$ where $e_n$ is the control error at step $n$, $t_s$ is a control period, and $k_p$, $k_I$ and $k_d$ are tuning coefficients. Note that the pure pursuit controller is a variant of the proportional controller.

Your task will be to implement an arbitrary path following approach. For your implementation, use provided template in aro_control package. The uploaded solution has to comply with actionserver-based API and publish velocity commands for robot on topic /cmd_vel (type: geometry_msgs/Twist). The current pose of the robot in a map can be obtained from transformations on topic /tf (an example provided in path_follower.py). The action server has to accept requests on /follow_path (type: aro_msgs/FollowPathAction). Your commands has to lead to safe path-following that will not result in a collision with obstacles. Your solution has to fulfill following requirements:

• all points on a path are visited with 0.5 m tolerance (robot have to follow the path even if it starts and ends at the same position),
• deviation from a reference path will not exceed 0.2 m,
• the end point of the path will be reached (with 0.2 m tolerance),
• path following process will be safely stopped after reaching the goal,
• path following process will not be slower by more than 30% in comparison to the reference solution,
• path following process will be safely stopped if the preemption request is received,

Otherwise, no requirements are imposed on a submitted solution.

Hints and possible improvements of the introduced control laws:

• adjust forward velocity based on the required angle to turn (even zero forward velocity can be advantageous in certain situations),
• do not apply $\omega = \frac{v}{R}$ for $v \to 0$,
• beware that the control of actuators is not precise and thus the robot's behavior can deviate from the expected behavior derived from applied control inputs,
• consider the frequency of the control loop.

The aro_control package provided in resources for this homework includes following files:

• scripts
  • path_follower.py - template for the path following script
  • path_follower_evaluator.py - script for local testing of the implemented solution
• launch
  • onlysim.launch - launch file for launching simulation and RViz with the path following visualizations
  • control.launch - launch file for launching path follower with parameters loaded from control.yaml file
  • evaluation.launch - launch file for local testing of the implemented solution
• config
  • control.yaml - config file loaded by control.launch
• evaluation
  • test_paths.csv - csv file containing paths for testing (loaded by local evaluation script)

The template file path_follower.py contains an implementation of the path following compatible with the rest of the pipeline. However, it generates random control inputs that do not meet the requirements of the assignment. Apart from the path following with poor performance, the template script also does not correctly handles all situations specified in the task assignment. All parts of the code that are expected to be changed are identified by the keyword TODO. However, the changes you may apply are not limited to these sections. You can update anything until your script fulfils the requirements specified in the Assignment section. The template file contains also several functions that you may find useful during the implementation of your task.

For testing your solution link the aro_control package into your workspace containing aro_sim, aro_slam and aro_msgs packages and build the workspace. Then you can run the whole pipeline for testing your solution by running four launch files:

• roslaunch aro_control onlysim.launch
• roslaunch aro_slam aro_slam.launch (or another launch file running your solution of HW_04)
• roslaunch aro_control control.launch
• roslaunch aro_control evaluation.launch

The evaluation script consequently sends path following requests with paths loaded from a file specified in evaluation.launch. The file with path definitions is the csv file with following format:

0.0,1.0
1.0,1.0
1.0,0.0
0.0,0.0
PATH_END,30.0
0.0,0.0
0.0,-1.0
-1.0,-1.0
PATH_END,12.5

Each line specifies x, y coordinates of a waypoint in the world frame. The keyword PATH_END identifies the end of a path and is followed by the expected time limit for the path following of a given path.

By running the aforementioned launch files you should see an RViz window similar to this:

Please submit path_follower.py and control.yaml (optional) in a single archive file. You can get 5 points for this task at maximum. Any solution achieving at least 3 points is considered to be acceptable. The automatic evaluation uses the realistic simulator to verify correctness of your solution. Your solution is evaluated using a set of paths similar to paths provided in test_paths.csv. The points obtained for the solution are proportional to number of successfully followed paths while fulfilling the requirements specified in Assignment section. Note that to decrease the evaluation time, the simulation is not restarted for every path in a test set. Therefore, if your solution performs exceptionally bad and it navigates the robot into the obstacle while following some path, it can negatively influence the evaluation process for the rest of the paths.