Introduction

This page describes a quick way to achieve basic autonomous driving in a terrain between two or more waypoints.

Creating simple terrain maps

Given a terrain information in a form of point-cloud, points-join utility can be used to create maps from a terrain information in the form of a point cloud. Consider following dataset

> # download dataset
> curl http://perception.acfr.usyd.edu.au/data/samples/riegl/rose.st/rose.st.ground.csv.gz | gunzip > ground.csv
> curl http://perception.acfr.usyd.edu.au/data/samples/riegl/rose.st/rose.st.nonground.csv.gz | gunzip > non-ground.csv

> # make sense of data
> view-points "ground.csv;fields=x,y,z,r,g,b" "non-ground.csv;fields=x,y,z,r,g,b"

A simplistic map can be made with no effort as follows:

> # assign graph vertex ids
> cat ground.csv | cut -d, -f1,2,3 | csv-paste - line-number > nodes.csv

> # make graph edges (simply densely mesh points in a given radius
> cat nodes.csv | points-join nodes.csv --radius 0.12 --all | csv-eval --fields=,,,a,,,,b --output-if "a != b" > edges.csv

> # view graph
> view-points "nodes.csv;fields=x,y,z;colour=white" "edges.csv;shape=line;colour=grey;fields=first,,second"

 

One easily can add more on top: filter out no-go zones, add or remove edges etc

Planning

Creating a plan on a terrain map

With distance as a cost function

A plan between two (or more) waypoints can be generated by running graph-search utility on a terrain map.

> # search for path between node with id 5000 to node with id 100000 (remember how we numbered the graph nodes using csv-paste above)
> graph-search --from 5000 --to 100000 --nodes "nodes.csv;fields=x,y,z,id" --edges "edges.csv;fields=,,,source,,,,target" > path.csv
> # view the result
> from=5000 ; to=100000 ; view-points "nodes.csv;fields=x,y,z;colour=grey;weight=1" "edges.csv;shape=line;colour=grey;fields=first,,second" <( cat nodes.csv | egrep ",$from$|,$to$" )";colour=yellow;weight=10;fields=x,y,z,label" <( cat path.csv | csv-eval --fields ,,z "z=z+0.25" )";shape=lines;colour=yellow" <( cat path.csv | csv-eval --fields ,,z "z=z+0.25" )";weight=3;colour=yellow"

Our path is fairly jagged. There are lots of smoothing methods that are relatively easy to implement. As a quick fix you could simply higher --radius value for points-join, by the price of higher computation time. Try e.g. points-join ... --radius 1.5; it takes longer, but the path is way more smooth:

With a custom cost function

The plan we got above is based on minimum distance. We could add cost to each edge in edges.csv . Then graph-search will use the cost instead of distance.

Assume, it is expensive for us to drive on the grass (because the gardener will charge us for damages).

> # quick and dirty: add to each vertex the amount of green in it (the formula for colour is tuned for demonstration only)
> cat ground.csv | csv-eval --fields ,,,r,g,b "t=(-1.3*r+2*g-1.3*b)*1.1+255" | cut -d, -f1,2,3,7 | csv-paste - line-number > nodes.with-cost.csv
 
> # get edges with amount of green as their cost
> time cat nodes.with-cost.csv | points-join nodes.with-cost.csv --radius 0.2 --all -v | csv-eval --fields=,,,,a,,,,,b --output-if "a != b" > edges.with-cost.csv
 
> # search path with cost
> graph-search --from 5000 --to 100000 --nodes "nodes.with-cost.csv;fields=,,,,id" --edges "edges.with-cost.csv;fields=,,,,source,,,,cost,target" > path.avoid-grass.csv
 
> # search path by distance
> graph-search --from 5000 --to 100000 --nodes "nodes.with-cost.csv;fields=x,y,z,,id" --edges "edges.with-cost.csv;fields=,,,,source,,,,,target" > path.by-distance.csv
 
> # view results
> from=5000 ; to=100000 ; view-points "nodes.with-cost.csv;fields=x,y,z,scalar;color=290:360,jet" <( cat nodes.with-cost.csv | egrep ",$from$|,$to$" )";colour=yellow;weight=10;fields=x,y,z,,label" <( cat path.avoid-grass.csv | csv-eval --fields ,,z "z=z+0.25" )";shape=lines;colour=yellow" <( cat path.avoid-grass.csv | csv-eval --fields ,,z "z=z+0.25" )";weight=3;colour=yellow" <( cat path.by-distance.csv | csv-eval --fields ,,z "z=z+0.25" )";shape=lines;colour=green;title=by-distance" <( cat path.by-distance.csv | csv-eval --fields ,,z "z=z+0.25" )";weight=3;colour=magenta"

As you see, the path by distance (coloured magenta) is almost a straight line, while path for avoiding grass (coloured yellow) tries to avoid the green areas, albeit not completely. If in the formula above "t=(-1.3*r+2*g-1.3*b)*1.1+255" you use a greater multiplier instead of 1.1 (e.g. 1.5), it will make driving on grass so prohibitive that you will see the path going around the lawn and avoiding greens completely.

This example does not demonstrate anything novel, it all are well-known decades-old algorithms. Instead, it demonstrates how just in three command lines you could build a reasonable drivable path on a terrain represented by a relatively arbitrary point cloud.

Modifying a plan

Moderating speed

control-speed utility sets the speed of each waypoint in the path based on its position in a curve.

turn operation calculates the angle at each waypoint with respect to its adjacent waypoints and assigns the speed according to given maximum lateral acceleration.

$ ( echo '0.0,0.0'; echo '0.3,0.3'; echo '0.6,0.6'; echo '0.6,0.9'; echo '0.6,1.2'; echo '0.9,1.2'; echo '1.2,1.2'; echo '1.5,0.9'; echo '1.8,0.6' ) > trajectory.csv
 
# moderate speed
$ control-speed turn --max-acceleration=0.5 --approach-speed=0.2 --fields=x,y --speed=1 < trajectory.csv > speed-turn.csv
 
# visualise with trajectory as blue and speed as z axis in yellow
$ view-points "trajectory.csv;fields=x,y;shape=lines;title=trajectory" \
    <( echo 0,0,begin )";fields=x,y,label;weight=8;color=red;title=origin" \
    "speed-turn.csv;fields=x,y,z;shape=lines;color=yellow;title=turn" 


control-speed decelerate operation moderates the sudden decrease in speed in the trajectory by a given deceleration.

 

$ control-speed decelerate --fields=x,y,speed --deceleration=0.5 < speed-turn.csv > speed-decelerate.csv
 
# visualise with speed as z-axis and orange color as the decelerated speed
$ view-points "trajectory.csv;fields=x,y;shape=lines;title=trajectory" <( echo 0,0,begin )";fields=x,y,label;weight=8;color=red;title=origin" \
    "speed-turn.csv;fields=x,y,z;shape=lines;color=yellow;title=turn" "speed-decelerate.csv;fields=x,y,z;shape=lines;color=orange;title=decelerate"

Handling sharp turns

By specifying --stop-on-sharp-turn or --pivot, control-speed can implement spot turn by outputting an extra waypoint with relative heading and no speed, for each sharp turn in the trajectory.

# stop on sharp turns
control-speed turn --max-acceleration=0.5 --approach-speed=0.2 --fields=x,y --speed=1 --pivot < trajectory.csv > speed-pivot.csv

# visualise with trajectory as blue and speed as z axis in yellow
$ view-points "trajectory.csv;fields=x,y;shape=lines;title=trajectory" \
    <( echo 0,0,begin )";fields=x,y,label;weight=8;color=red;title=origin" \
    "speed-pivot.csv;fields=x,y,z;shape=lines;color=yellow;title=turn"

Customizing desired heading

As can be inferred from previous section, spot turn to a specific heading at a waypoint can be achieved by copying that waypoint, changing the duplicates speed to zero, heading field to the desired heading and indicating absolute heading.

# output a plan from trajectory with fields x,y,speed,heading,is_absolute with speed=1 and both heading and is_absolute are set to 0
> csv-paste - 'value=1,0,0' < trajectory.csv
0.0,0.0,1,0,0
0.3,0.3,1,0,0
0.6,0.6,1,0,0
0.6,0.9,1,0,0
0.6,1.2,1,0,0
0.9,1.2,1,0,0
1.2,1.2,1,0,0
1.5,0.9,1,0,0
1.8,0.6,1,0,0

# Spot turn at (0.6,1.2), by copying it and setting 0 speed and 1.5 absolute heading. Make sure to decelerate to that point
> cat <<EOF > plan.csv
0.0,0.0,1,0,0
0.3,0.3,1,0,0
0.6,0.6,1,0,0
0.6,0.9,0.6,0,0
0.6,1.2,0.3,0,0
0.6,1.2,0,1.5,1
0.9,1.2,1,0,0
1.2,1.2,1,0,0
1.5,0.9,1,0,0
1.8,0.6,1,0,0
EOF

 

Control

Turning plan waypoints into control commands for a vehicle

control-follow-waypoints takes the plan and the live navigation data and outputs the velocity, turn rate, and (for omni-directional vehicles) heading required to follow that plan.