This example builds on the Pioneer in Gazebo, GPS/IMU Navigation, MATLAB OOP Waypoint Guidance example with the following extensions:

- We provide a brief overview simple path-following using line-of-sight (LOS) guidance
- We generalize our object-oriented implementation using function handles so that we can re-use the same MATLAB object and simply re-assign the guidance algorithm.

# Line-of-Sight Path-Following

"The nonlinear LOS law is widely used to solve practical path-following problems of marine vehicles due to its simplicity and intuitiveness: it imitates a helmsman steering the vessel toward a point lying at a constant distance ahead of the ship along the desired path"^{1
}
The image below provides an illustration of the approach.

The objective is to move from the red point to the green point following the line (minimizing the across-track error). If we know the location of both endpoints and the location of the robot, we can find the perpendicular line that connects the robot's location to the line, illustrated by the blue dotted line. The "look ahead distance" (d_{los}) is a user-selected, tunable parameter that defines the tradeoff between heading toward the line and heading for the end point (green marker). With the look ahead distance we can find the location of the yellow target point. Based on this goal point we can find the yaw angle (heading) to move toward that target (\theta_t).

To arrive at a Twist command we need to define an angular and linear velocity. The angular velocity is a function of the error between (\theta_t - \theta_z) - for now this will just be proportional. The linear velocity is typically a constant when we are far form the end point and then decreases as we approach the end of the line.

### MATLAB LOS Algorithm Implementation

An implementation of this simple guidance algorithm is provided as a MATLAB function called `pioneeer_los`

which is available in the nre_gazebo ROS package: https://github.com/bsb808/nre_gazebo/blob/master/matlab/pioneer_los.m

We can visualize the implementation by creating a grid of locations around a line and running this function.

In this figure the line to follow is between the green and red markers. The arrows represent the linear velocity and angular velocity of the resulting commands from the `pioneer_los`

function. For this example the look-ahead distance was set to 10 m. We can also see that the velocities are constant for most of the space - at 1.0 m/s which is the maximum velocity specified in the algorithm - but near the end point of the line the velocity is reduced, as evident by he smaller magnitude arrows.

# MATLAB ROS Implementation

In this example we use the same basic structure for the object-oriented ROS controller as described in Pioneer in Gazebo, GPS/IMU Navigation, MATLAB OOP Waypoint Guidance with one improvement.

In the previous example we created the `pioneerwaypointclass`

as a MATLAB Class. This class object took care of the asynchronous ROS communication by registering multiple publishers/subscribers. This ROS communication and logic (the 'plumbing')was about 90% of the code. The actual waypoint algorithm was a relatively small single function.

In this example we'll split the ROS plumbing from the guidance algorithm, that way we can reuse the plumbing for a variety of different algorithms. We can do this by defining the `guidance_function`

property of the new `pioneerguidanceclass`

. We can then define (or redefine) the this object property as a function handle, where the function implements the particular guidance algorithm we are using.

In this example our MATLAB code has three parts:

- The implementation of the line-of-sight path-following guidance algorithm in the single
`pioneer_los`

function: https://github.com/bsb808/nre_gazebo/blob/master/matlab/pioneer_los.m - The general purpose MATLAB class definition for asynchronous ROS communication: https://github.com/bsb808/nre_gazebo/blob/master/matlab/pioneerguidanceclass.m
- The
`pioneer_los_oop`

script that instantiates the class object, and then queries the class object for status updates: https://github.com/bsb808/nre_gazebo/blob/master/matlab/pioneer_los_oop.m

# Example

Here is an working example. The code for this example is in the nre_gazebo ROS package: https://github.com/bsb808/nre_gazebo

### Gazebo Setup

We'll start Gazebo with a world that includes visual markers to show the line endpoints...

roslaunch nre_gazebo p3at_gazebo_one.launch initX:=90 initY:=20 initYaw:=0

This should generate and initial view like this...

### MATLAB Execution

Now in MATLAB you should be able to simply run the `nre_gazebo/matlab/pioneer_los_oop.m`

script and see the robot follow the lines...

This works well, but you'll notice that on the 90 degree turns between consecutive lines that the robot goes unnecessarily far from the line. We might add a heuristic to the LOS algorithm so that if the angular error is large, we turn in place rather than turning and driving forward simultaneously. We can add this logic at the end of the `pioneer_los`

function like so...

% Logic for sharp turns if abs(hdgError) > 0.5 linvel = 0; end

Now we can see a bit different behavior in our path following...