Skip to end of metadata
Go to start of metadata

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.

Run the example

  1. Compile your code.
  2. Upload it in the LCC.
  3. Select vehicle 4 (simulated mode).
  4. Hit "Deploy". You will see a trail which builds and erases from point to point. This is your trajectory. Now "kill" the program (press the "Kill" button in the LCC) and move the simulated vehicle close to the trajectory by dragging it to a close point to the figure eight trajectory. A second trail will be drawn which is the trajectory for your vehicle to get the figure eight trajectory.
  5. Once the vehicle is on the figure eight trajectory "deploy" your program again and the vehicle will drive continously in endless loop.
  6. To end the program hit "Kill" again.


  • No labels