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/two_vehicles_drive

Goal

Two vehicles will drive in a basic circle with a radius of 1m. The center of the circle of vehicle 1 will be at (x,y) = (1,2) and the center of vehicle 2 will be at (x,y) = (4,2). The circles shall be driven continously and counterwise to each other.

Initialisation

Define the IDs of the vehicles you want to drive. Read the IDs 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 and speed

Define four points of your circle on a coordinate system.

Define the speed vector of the vehicle for each point for the entire trajectory. When you define vx and vy remember that vges = sqrt(vx^2 +vy^2) (as it is a vector). You use this vector to define the direction for your vehicle, so where it has to steer next. Always assign at least a small value to both vectors to ensure the correct direction. Remember that your are defining a physical system, so avoid sharp edges.

Draw the vecotrs on each point of your trajectory:

vehicle 1vehicle 2

Now implement your points and speed values using the predefined vetors trajectory_px and trajectory_py for your points of the trajectory and trajectory_vx and trajectory_vy for the speed at each point. The speed is given in m/s. Set the center of the circle for vehicle 1 at (x,y) = (1.25, 2.9) and the center of vehicle 2 at (x,y) = (3.1,1.1).

Segment duration

For the segment duration keep in mind that speed, time and waylength have to fit. Verify your speed/way/duration with vges =ssegment/tsegmentduration.

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 3 and 12 (simulated mode).
  4. Hit "Deploy". You will see a trail which builds and erases from point to point. This is your trajectory. Now move the simulated vehicles close to the trajectory by dragging them to a close point to the circle trajectory. A second trail will be drawn which is the trajectory for your vehicle to get the circle trajectory.
  5. Once the vehicle is on the circle trajectory it will drive continously in endless loop.
  6. To end the program hit "Kill".





  • No labels