Go to start of banner

# Diagonal Figure Eight Example

You find the source code for our examples in the software-folder:

##### C++

https://github.com/embedded-software-laboratory/cpm_lab/tree/master/high_level_controller/examples/cpp/diagonal_figure_eight

# Goal

One vehicle drives a diagonal figure eight on the ground, unattatched of the map. The supporting points are (x,y) = (-1,-1), (0,0), (1,1). # Initialisation

Define the ID of the vehicle you want to drive. Read the ID in from the LCC using the function cmd_parameter_ints() from our cpm library.

Initialize a DDS Writer for the RTI DDS Service to ensure that position and speed will be sent to the simulation and the vehicle. Write on the topic `vehicleCommandTrajectory` with datatype `VehicleCommandTrajectory`. The topic's name is the name of the datatype with a lowercase letter.

# Giving direction, speed and timestamp

Define your trajectory points for x and y according to the goal of this tutorial. Like in basic circle, the shape is to be driven continously, so the vector you define has to be set as a "round-trip": (0,0) → (-1,-1)->(0,0)→(1,1) →(0,0) and then again (-1,-1) and so on.

Define your speed in x direction and y direction. Remember that the speed is now a vector vges from vx and vy (see Basic Circle Example). For the segment (-1,-1)→(0,0) you should consider: vmax = 1,3 m/s. So vges  =sqrt (vx2 + vy2) = 1,3 m/s =sqrt(2) * v|vx=vy → vx=vy= 0,928 m/s.

Always keep in mind that the speed vector of one point influences the shape of the curve of the next point.

Define the timestamp. Start with 0s. The next timestamp is about the length of a half circle with radius 1 with the given total speed at t1= s1/vtotal = (2 * pi * 1 m)/2 /1,3 m/s  = pi m/1,3m/s = 2,416s.

As each waysegment is half a circle we can multiply t1 correspondingly.

# Sending the trajectory information to the vehicle

All data are sent to the vehicle using the writer-function from the beginning in the form of

`{point_x_position;point_y_position;velocity_x_direction;velocity_y_direction; timestamp;}`

where each value is a scalar.