Your task will be to finish an implementation of path following node responsible for generation of actuator inputs leading to safe following of a given path.

Responsible lecturer: Vít Krátký (kratkvit@fel.cvut.cz)

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 efficient approach, which is often used for differential wheeled robots, is a “pure pursuit law” which is based on 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 $\mathbf{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 following curve $k$ going through position $\mathbf{P_d}$.

Considering a constant control inputs, curve $k$ that leads the robot through desired position $\mathbf{P_d}$ is part of the circle with diameter $\mathbf{R}$. From the presented scheme, we can derive that $\mathbf{R}$ can be computed based on equation $$ \mathbf{R} = \frac{{d_l}^2}{2\Delta p}, $$ where $\Delta p$ stands for a distance between $\mathbf{P_d}$ and its orthogonal projection to line coincident with robot's heading $\varphi$. With a given constant velocity $v$ and computed radius $\mathbf{R}$, the desired angular velocity $\omega$ that will result in a following of curve $k$ is given by $$ \omega = \frac{v} {\mathbf{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 based on a given path and current position of the robot,
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 $\mathbf{P_d}$ and the current position of the robot. The control error $e$ is then given as a deviation of the current heading $\varphi$ 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 script path_follower.py in aro_exploration package (folder nodes/control) and a configuration file control.yaml (folder config). 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 is advantageous in certain situations),
• do not apply $\omega = \frac{v}{\mathbf{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 template file aro_exploration/nodes/control/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 comment # TODO HW 05. However, the changes you may apply are not limited to these sections. You can update anything until your script fulfills the requirements specified in the Assignment section. File aro_exploration/src/aro_control/utils.py contains several functions that you may find useful during the implementation of your task.

The configuration file control.yaml located in aro_exploration/config folder contains values of parameters that are loaded by script path_follower.py. The configuration file is automatically passed to the script when starting the task using recommended launch files for local testing and evaluation and it overrides default values of your parameters specified in script path_follower.py.

You can run your solution including RViz visualization by running

roslaunch aro_exploration aro_control_sim.launch

• simulation of Turtlebot robot,
• your implementation of robot's state estimation from aro_exploration package (HW 04),
• your implementation of path following provided in path_follower.py,
• and preconfigured RViz window visualizing the robot in the environment and mission-related statistics.

Local evaluation

Local evaluation of your solution can be run using

roslaunch aro_exploration aro_control_evaluation.launch

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 file you should see an RViz window similar to this:

Once the evaluation is finished, the script prints results of the evaluation in the command line. The output of the evaluation for correct implementation should look like:

In this table, goal reached indicates whether a goal was reached with required precision, dev ok indicates whether maximum deviation from path was not exceeded, column time ok indicates whether the time of the following does not exceed a limit for particular paths, column follow time shows the time of following for individual paths, time diff indicates the difference of following time to time limit (negative numbers stand for the time below the limit, positive numbers stand for time over the limit), columns avg_dev, min_dev, and max_dev show the average, minimum, and maximum deviation from desired path, respectively.

 roslaunch aro_exploration aro_control_evaluation.launch ground_truth_control:=true

If you want to run your implementation of the path following in a separate command line window in order to separate the command line outputs, you can run the whole simulation including robot's localization and local evaluation using command

roslaunch aro_exploration aro_control_evaluation.launch run_control:=false

roslaunch aro_exploration aro_control.launch

To suppress the outputs of your solution of HW04 to command line you can change the logger level for icp_slam node by calling service

rosservice call /icp_slam/set_logger_level "logger: 'rosout' level: 'error'"

Run script aro_exploration/homeworks/create_hw05_package.sh to create an archive containing your implementation (path_follower.py and control.yaml) and upload it to upload system. 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_students.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.