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 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 straight forward 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 moves forwards 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$.

The 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}. $$

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 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 proportional controller.

Possible improvements and hints:

• adjust forward velocity based on the required angle to turn (even zero forward velocity can be advantageous in certain situations)
• beware that the following of the path is usually not precise
• consider the frequency of control loop

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:

• deviation from reference path will not exceed 0.2 m
• the end point of 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 reference solution
• path following process will be safely stopped if the preemption request is received

Otherwise, no requirements are imposed on submitted solution.

The resources contain the aro_control package including following files:

• scripts
  • path_follower.py - template for the path following script
  • path_follower_evaluator.py - script for local testing of path follower
• launch
  • onlysim.launch - launch file for launching simulation and rviz with 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 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 implementation of path following compatible with the rest of the pipeline. However, it generates random control inputs that does not meet requirements of the assignment. Apart from the path following with poor performance, the template script also does not correctly handles all situations specified in task assignment. All parts of the code that are expected to be changed are identified by keyword TODO. However, the changes you want to apply are not limited to these sections. You can update anything until your script fulfills the requirements specified in 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 and aro_slam packages and build the workspace. Then you can run the whole pipeline for testing your solution by running four launch files:

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

The evaluation script consequently sends path following requests with path loaded from file specified in evaluation.launch. The file with path definitions is the csv 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 key word PATH_END identifies the end of a path and is followed by the expected time limit for 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. 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 the first path, it can negatively influence the evaluation process for the rest of the paths.