Build A High Level Control Node

The joystick commander project demonstrates how to control a vehicle with a high level control node written using the PolySync Core C API.

The joystick commander populates and publishes control commands to the PolySync bus. One of the supported drive-by-wire systems subscribes to the control commands and issues the actuation commands for steering, brake, and throttle.

The DBW interfaces enforce strict runtime requirements for how the data is formatted, and how frequently command messages should be received.

The joystick commander project can be used as a reference to understand how high level control nodes meet the DBW interface requirements.

1. Control node requirements

  • Vehicle control must be enabled at the system level
  • Control messages must be published to the PolySync bus at 50Hz or greater
  • The steering, throttle, and brake messages must be published, even if you only want to actuate part of the system
  • The GUID of the DBW interface destination node must be set within the command message

2. The PolySync joystick commander project

The joystick commander project lives in the PolySync GitHub.

This control node application translates the input of the gamepad as valid steering, brake, and throttle commands.

2.1 Hardware requirements

  • Joystick device: Logitech Gamepad F310
  • An ECU with a USB port
  • ECU with PolySync Core installed

2.2 Install dependencies

$ sudo apt-get install libglib2.0-dev libsdl2-dev

2.3 Build and run the control node

$ cd joystick_commander
$ make
$ ./bin/polysync-joystick-commander 

The USB joystick must be connected when you start the control application polysync-joystick-commander.

Controls are disable until the Start button is manually pressed on the physical joystick.

Once controls are enabled the control node will begin translating the joystick position and button presses as control inputs.

2.4 Publising control messages

File: joystick_commander/src/node.c
static void update_loop(
        ps_node_ref const node_ref,
        commander_s * const commander )
{
    int ret = DTC_NONE;
    ps_timestamp now = 0;


    // get the amount of time since our last update/publish
    const ps_timestamp time_since_last_publish =
            get_time_since( commander->last_commander_update, &now );

    // if we have a valid destination node GUID
    if( commander->dest_control_node_guid != PSYNC_GUID_INVALID )
    {
        // only update/publish at our defined interval
        if( time_since_last_publish >= NODE_UPDATE_INTERVAL )
        {
            // update commander, send command messages
            ret = commander_update(
                    node_ref,
                    commander );
            if( ret != DTC_NONE )
            {
                psync_node_activate_fault( node_ref, ret, NODE_STATE_FATAL );
            }

            commander->last_commander_update = now;
        }
    }
    else
    {
        ret = messages_get_discovered_guid(
                &commander->dest_control_node_guid );
        if( ret != DTC_NONE )
        {
            psync_node_activate_fault( node_ref, ret, NODE_STATE_FATAL );
        }

        if( commander->dest_control_node_guid != PSYNC_GUID_INVALID )
        {
            psync_log_info( "got valid response from control node GUID 0x%016llX (%llu)",
                    (unsigned long long) commander->dest_control_node_guid,
                    (unsigned long long) commander->dest_control_node_guid );

            ret = messages_unregister_response_subscriber(
                    node_ref,
                    &commander->messages );
            if( ret != DTC_NONE )
            {
                psync_node_activate_fault( node_ref, ret, NODE_STATE_FATAL );
            }
        }
        else
        {
            // check for re-enumerate button action
            ret = commander_check_reenumeration(
                    node_ref,
                    commander );
            if( ret != DTC_NONE )
            {
                psync_node_activate_fault( node_ref, ret, NODE_STATE_FATAL );
            }
        }
    }

    // sleep for 1 ms to avoid loading the CPU
    (void) psync_sleep_micro( NODE_SLEEP_TICK_INTERVAL );
}
2.4.1 Set message publish rate

The application must publish data to the bus at 50Hz or greater. This is acheived in the joystick commander by comparing the last publish time to a predefined node update interval.

if( time_since_last_publish >= NODE_UPDATE_INTERVAL )

See the time_since_last_publish function for an example of how to calculate the time since the last message was published to the bus.

2.4.2 Destination node GUID

Within the update_loop function the joystick commander node enumerates a list of detected node GUIDs. Using the GUID and the PolySync APIs, the application can determine the node type to associate a GUID to the DBW interface node.

See the messages_get_discovered_guid function for an example of how to filter for a specific node GUID.

2.4.2 Populate and publish the messages

The mandatory command messages for steering, brake, and throttle are published by commander_update.

  • ps_platform_brake_command_msg
  • ps_platform_steering_command_msg
  • ps_platform_throttle_command_msg

In this example we also publish commands for the gear and turn signal.

File: joystick_commander/src/commander.c
int commander_update(
        ps_node_ref node_ref,
        commander_s * const commander )
{
    int ret = DTC_NONE;
    unsigned int disable_button_pressed = 0;
    unsigned int enable_button_pressed = 0;


    if( commander == NULL )
    {
        ret = DTC_USAGE;
    }

    // safe state
    if( ret == DTC_NONE )
    {
        ret = commander_set_safe( commander );
    }

    // update joystick
    if( ret == DTC_NONE )
    {
        ret = jstick_update( &commander->joystick );
    }

    // get 'disable-controls' button state
    if( ret == DTC_NONE )
    {
        ret = get_disable_button(
                &commander->joystick,
                &disable_button_pressed );
    }

    // get 'enable-controls' button state
    if( ret == DTC_NONE )
    {
        ret = get_enable_button(
                &commander->joystick,
                &enable_button_pressed );
    }

    // only disable if both enable and disable buttons are pressed
    if( (enable_button_pressed != 0) && (disable_button_pressed != 0) )
    {
        enable_button_pressed = 0;
        disable_button_pressed = 1;
    }

    // send command if a enable/disable command
    if( disable_button_pressed != 0 )
    {
        ret = commander_disable_controls(
                node_ref,
                commander );
    }
    else if( enable_button_pressed != 0 )
    {
        ret = commander_enable_controls(
                node_ref,
                commander );
    }
    else
    {
        // publish brake command continously
        if( ret == DTC_NONE )
        {
            ret = publish_brake_command(
                    node_ref,
                    &commander->joystick,
                    commander->messages.brake_cmd );
        }

        // publish throttle command continously
        if( ret == DTC_NONE )
        {
            ret = publish_throttle_command(
                    node_ref,
                    &commander->joystick,
                    commander->messages.throttle_cmd );
        }

        // publish steering command continously
        if( ret == DTC_NONE )
        {
            ret = publish_steering_command(
                    node_ref,
                    &commander->joystick,
                    commander->messages.steering_cmd );
        }

        // publish gear command on button event
        if( ret == DTC_NONE )
        {
            ret = publish_gear_command(
                    node_ref,
                    &commander->joystick,
                    commander->messages.gear_cmd );
        }

        // publish turn signal command on button event
        if( ret == DTC_NONE )
        {
            ret = publish_turn_signal_command(
                    node_ref,
                    &commander->joystick,
                    commander->messages.turn_signal_cmd );
        }
    }


    return ret;
}

Before commander_update publishes a message it verifies there are no error conditions. If no errors are found it publishes an instance of each control message.

The message gets populated in the publish_*_command functions. Publishing the brake command is shown below.

File: joystick_commander/src/commander.c
static int publish_brake_command(
        ps_node_ref node_ref,
        joystick_device_s * const jstick,
        ps_platform_brake_command_msg * const msg )
{
    int ret = DTC_NONE;
    double brake_setpoint = 0.0;


    ret = get_brake_setpoint(
            jstick,
            &brake_setpoint );

    if( ret == DTC_NONE )
    {
        msg->brake_command = (DDS_float) brake_setpoint;
        msg->enabled = 1;
        msg->brake_command_type = BRAKE_COMMAND_PEDAL;

        if( brake_setpoint >= BRAKES_ENABLED_MIN )
        {
            // enable on-off indicator
            msg->boo_enabled = 1;
        }
    }

    if( ret == DTC_NONE )
    {
        ret = psync_get_timestamp(
                &msg->header.timestamp );
    }

    if( ret == DTC_NONE )
    {
        ret = psync_message_publish(
                node_ref,
                (ps_msg_ref) msg );
    }


    return ret;
}

The function get_brake_setpoint reads and returns the position of the joystick.

If no error is retuned, the brake position that is read from the joystick is populated in the control message.

The message gets published to the PolySync bus by the function psync_message_publish. If the message was successfully published a DTC_NONE will be returned.

The steering and throttle command message input is constructed in a similar fasion, by reading the gamepad button and trigger positions.

To autonomously send control commands to a vehicle, replace the call to the get_brake_setpoint fuction to the output of your autonomous control algorithm.