Vehicle Control Tutorial

In this tutorial we will demonstrate how the PolySync bus, combined with PolySync’s publish/subscribe architecture, can be used to control a vehicle in real time.

1. The PolySync parrot projects

Parrot Controller and Parrot Visualizer live in our public C examples repo. After downloading the repo manually or using git to clone it, open two terminals. In the first terminal, navigate to the “parrot_controller” project in the location shown below:

PolySync-Core-C-Examples/parrot_controller
├── include
│   ├── common.h
│   ├── gl_headers.h
│   ├── grid.h
│   ├── gui.h
│   ├── ps_interface.h
│   ├── render.h
│   ├── vehicle_control.h
│   └── waypoint.h
├── Makefile
└── src
    ├── grid.c
    ├── gui.c
    ├── parrot_controller.c
    ├── ps_interface.c
    ├── render.c
    ├── vehicle_control.c
    └── waypoint.c

In the second terminal, open the “parrot_visualizer” project:

PolySync-Core-C-Examples/parrot_visualizer
├── include
│   ├── common.h
│   ├── driver_vehicle.h
│   ├── gl_headers.h
│   ├── gui.h
│   ├── ps_interface.h
│   ├── render.h
│   └── sliding_filter.h
├── Makefile
├── res
│   └── parrot.png
└── src
    ├── driver_vehicle.c
    ├── gui.c
    ├── parrot_visualizer.c
    ├── ps_interface.c
    ├── render.c
    └── sliding_filter.c

1.1 Install dependencies

$ sudo apt-get install libglib2.0-dev freeglut3-dev libsdl2-dev libsdl2-image-dev libpng12-dev 

1.2 Make the PolySync parrot projects

Using the existing makefiles, make both projects and then run them.

Parrot Windows

2. Run the waypoint controller

Begin manually specifying waypoints by pointing and clicking on a few different locations on the “parrot_controller” example. Waypoints become visible where clicked, and the vehicle in the “parrot_visualizer” example will start to move towards the closest one. You are now driving your first autonomous vehicle in PolySync.

Parrot Driving

3. Using Trace to view messages on the bus

Begin by opening PolySync Studio:

$ polysync-core-studio

Then, click on Trace in the “Launch Plugin” right-hand sidebar. Trace should open up as shown below.

Studio Trace

The drop down list in the left-hand sidebar contains all of the message types that have been seen on the bus . It will update dynamically. Select a name for the Trace window in the text box above the drop down list. Once a message type and name for the Trace is selected hit “Start Trace.”

Now that Trace is running, you can create some waypoints by clicking in the parrot_controller example.

Trace Control

For a deeper understanding of what is going on here take a look below. The source file “vehicle_control.c” is where the pure pursuit algorithm lives that does the vehicle control.

PolySync-Core-C-Examples/parrot_controller
└── src
    └── vehicle_control.c

The function below is called by a loop in main. It receives the list of current waypoints which have been selected in the GUI. It also receives the user_data structure which contains the current position of the vehicle (parrot) on the screen. This vehicle position is continuously updated by the parrot-visualizer example which estimates the vehicles position based on steering and throttle messages. The parrot-visualizer example then loads this estimated position into a ps_platform_motion_msg and sends it out on the PolySync bus. The below operation, using the position information that has been received and processed, calls the “findNextWaypoint” function which determines what is the current waypoint goal based on the queue of waypoints (and also eliminates the current goal waypoint if it has been reached in this iteration). Once the waypoint goal is known, this is passed to the pure pursuit algorithm. Take a look at the next code snippet for an explanation of how this works.

void send_psync_messages_for_vehicle_control( node_data_s * user_data,
        waypoint_s * waypoints )
{
    // local vars
    node_data_s     *node_data  = NULL;

    // cast
    node_data = ( node_data_s* ) user_data;

    // ignore if not valid
    if( ( node_data == NULL ) || ( waypoints == NULL ) )
    {
        return;
    }

    waypoint_s nextWaypoint;

    if( findNextWaypoint( waypoints, &nextWaypoint, node_data->current_vehicle_position ) )
    {
        return;
    }

    double steeringAngle = 0;

    if( calculate_steering_angle_based_on_goal_waypoint(
            &nextWaypoint,
            node_data->current_vehicle_position,
            &steeringAngle ) )
    {
        return;
    }

    publish_steering_command( node_data->node, steeringAngle );    
    publish_throttle_command( node_data->node, DEFAULT_THROTTLE );
}

This function calculates the angle between the current heading of the vehicle and the angle from the vehicle’s current position to the goal waypoint. It then determines the error between these two angles, and uses this to calculate the lateral error of the vehicle from the goal path. Multiplied by a steering control factor this is applied to steering the vehicle.

int calculate_steering_angle_based_on_goal_waypoint(
        waypoint_s * goalWaypoint,
        waypoint_s currentPosition,
        double * steeringAngle )
{
    if( ! goalWaypoint->valid )
    {
        return 1;
    }

    double vehicleToWaypointAngle = calculate_angle_between_coordinates(
            currentPosition.x,
            currentPosition.y,
            goalWaypoint->x,
            goalWaypoint->y );

    double errorInHeading = calculate_smallest_interior_angle(
            currentPosition.heading*RAD2DEG,
            vehicleToWaypointAngle*RAD2DEG );    
    double lateralError = sin( errorInHeading*DEG2RAD );

    ( *steeringAngle ) = -STEERING_CONTROL_FACTOR*lateralError;

    return 0;
}

Once you have the desired steering angle, and desired throttle value, these are packaged up and published to the PolySync bus in the below.

    publish_steering_command( node_data->node, steeringAngle );

    publish_throttle_command( node_data->node, DEFAULT_THROTTLE );

These messages are received and processed by the parrot_control example, then used to estimate the position of the “vehicle” closing the loop.

Conclusion

Congratulations! You have now successfully controlled your first PolySync autonomous vehicle.