Getting Started

 
Getting Started with PolySync Core
Welcome to PolySync. Let's get started by understanding the basic concepts, installing the software, and running your first application node.

Introduction

Welcome To The PolySync Help Center

Thanks for joining us! We built PolySync after working with the world’s most advanced autonomous driving teams and realizing the need for faster app creation. We wanted the experience of autonomy system development to feel more like mobile app development; fast backend assembly that leaves you the time to focus on your algorithms.

PolySync is an autonomy operating system consisting of a middleware and set of software API services that are common across autonomous driving applications. This allows you to build, test, and deploy safe driverless vehicle applications quickly.

PolySync accomplishes this by separating the application layer from the underlying Operating System (OS) and hardware, making it more consistent to build and much more portable. This means that you can spend more time focusing on perfecting your algorithms.

Avoiding the bottleneck

The thing about autonomy systems is that they need to be able to handle massive amounts of data asynchronously and perform complex operations in real-time. This is where our distributed middleware steps in, acting as a way to dole out tasks to an unlimited number of computers across your network. This keeps your family of computers in a harmonious state of seamless communication.

Speaking of seamless communication

PolySync’s Abstracted data model takes care of communication by providing an abstraction layer that separates data consumers from data producers. It does this through a set of common messages that are broadcast and received to the PolySync bus through a publisher/subscriber model. The data living within each message is also in the vehicle-centered reference coordinate frame and is (by default) time stamped with UTC time. Another great aspect of PolySync communication is our shared memory platform. If you’re looking for lightning fast inter-process communication, this is where you’ll find it.

Installation Walkthrough

This article walks you through installing PolySync onto your device.

1. Download PolySync Core

Download PolySync Core from the download center to a local machine that meets the supported architecture requirements. You will need to accept the license agreement before you can download PolySync Core.

The PolySync Core release package must match your ECUs architecture and operating system. Contact support help@polysync.io to get access to a specific release package.

2. Migrating from earlier versions of PolySync

2.1 Prepare your files for migration

PolySync has provided a migration script for those of you who are upgrading from a pre-release version to PolySync Core 2.0.9.

The migration script is going to look in the default location /usr/local/polysync/ $PSYNC_HOME for the existing install and do two things:

  • Imports the configuration and logfile sessions that are currently in $PSYNC_HOME to the expected locations in $HOME/.local/share/polysync
  • Creates a backup tarball of the existing installation
    • The backup tarball can be copied to any Ubuntu 16.04 machine and the migration script will place the files where they’re expected for PolySync Core 2.0.9
    • Note: the migration script will delete the contents of $PSYNC_HOME after creating the backup tarball

The migration script, polysync-prerelease-migrate, ships with the PolySync Core 2.0.9 release package.

2.1.1 Run the migration script on the pre-release host
$ cd ~/Downloads
$ tar xf polysync-core_2.0.9.tar.gz
$ cd polysync-core_2.0.9
sudo ./polysync-prerelease-migrate

The migration script will create a backup tarball file in the /opt/ directory with a timestamp, for example /opt/polysync-backup-1476514517.tar.gz.

The migration script needs to be run once more on the new machine, but this time we will supply a command line argument that is the path to the backup tarball that was created in the previous step.

$ sudo ./polysync-prerelease-migrate /opt/polysync-backup-<your-UTC-timestamp-here>.tar.gz

The migration script will decompress the archive in a temporary location and then copy the configuration and logfile files to the expected locations on the system.

The script can take some time, especially if there were large logfile sessions on the pre-release host.

Note: if the migration script detects an existing SDF on the new system, it will not overwrite the newer SDF in $HOME/.local/share/polysync/config with the SDF from the original machine.

To install the original SDF on the new system, extract the SDF file from the polysync-backup-*.tar.gz tarball and copy it to the new location:

$ cd ~/Downloads
$ tar xf polysync-backup-<your-UTC-timestamp-here>.tar.gz
$ cp polysync/db/psync.sdf ~/.local/share/polysync/config/

Now that your pre-release files have been migrated to your new system you can install PolySync.

2.2 Moving your license to a new host

On your pre-release host run:

$ polysync-license-tool -a


*****************************
*** PolySync License Tool ***
API Build Version: 2.0.8-1478129437

Host ID: '(Ethernet) xxxxxxxxxxxx'

[...]
License status: valid
*****************************

On your new-host install the new version and run polysync-license-tool -a

Send mail to help@polysync.io telling them you are migrating to a new host. Be sure to include your: Host ID: '(Ethernet) xxxxxxxxxxxx' from your pre-release and new host systems.

3. Installing PolySync Core

If you are upgrading from a pre-release version of PolySync follow: Migrating files

After you have downloaded the PolySync Core tar file, open a terminal and navigate to the directory where it has been downloaded.

$ cd ~/Downloads
$ tar xf polysync-core_2.0.9.tar.gz
$ cd polysync-core_2.0.9
$ source ./install.sh    #  You will be prompted for your sudo password.

It can take up to 15 minutes for the installation to complete.

The install location, also known as the PolySync Core Home directory, is located at /usr/local/polysync and is denoted with the bash variable $PSYNC_HOME.

4. Activating the license

To download an existing license from the server when upgrading or reinstalling PolySync run the license tool with just the ‘-a’ flag.

$ polysync-license-tool -a


*****************************
*** PolySync License Tool ***
API Build Version: 2.0.9-1486398127

Host ID: '(Ethernet) xxxxxxxxxxxx'

[...]
License status: valid
*****************************
  • Your output should look similar to the output above.

If you do not have a valid license or are having problems activating your license: Contact support help@polysync.io. Be sure to include your: Host ID: '(Ethernet) xxxxxxxxxxxx'

4.1 Known License Error

If you see this error when running the license tool:

$ polysync-license-tool -a

*****************************
*** PolySync License Tool ***
API Build Version: 2.0.9-1486943244

[...]
2017-02-13 14:41:29.02s ERROR  [polysync-license-tool-0] - adding trusted storage license source returned: 1879048255
2017-02-13 14:41:29.02s DEBUG  [polysync-license-tool-0] - trusted storage based licensing may not be available
License status: invalid
To request a license, email help@polysync.io with your machines unique Host ID: '(Ethernet) xxxxxxxxxxxx'
*****************************

You will need to run the following command to clear your license cache:

rm ~/.local/share/polysync/license/am*

5. Start PolySync manager service

We are going to start the polysync-core-manager as a service. It will start the predefined nodes in the System Design File (SDF) with the command:

$ sudo service polysync-core-manager start

You can see the system SDF by opening the SDF Configurator

  • It defines the PolySync runtime:
    • All hosts and nodes in the system
    • What sensors are connected to the ECU, or
    • What sensors will be replaying data

Open the SDF Configurator with this command:

$ polysync-core-sdf-configurator

Note that while in the replay state, the system is referencing the replay SDF .

Configuring A PolySync Runtime

Single Host Setup

PolySync IP address

When using a static network, the DDS Discovery feature will search for other hosts through the defined Ethernet interface. If other PolySync hosts exist on the network the PolySync bus will be shared, which means other host’s and node’s data will be visible, and vice versa. The proper way to connect two or more ECUs is through a distributed host set up.

Set Address

For a single host system with a static IP address, the following command can be used:

$ polysync-core-manager -s xxx.xxx.xxx.xxx

This updates the low-level network configuration file telling PolySync which IP address is associated to the PolySync bus.

Feel free to use the default IP address: 127.0.0.1, or select a static IP address from a private network.

For single host systems it is highly recommended to use the loopback interface with the IP address 127.0.0.1.

$ polysync-core-manager -s 127.0.0.1

Multicast

One requirement for PolySync nodes to discover each other on the loopback interface is to enable multicast.

$ sudo ifconfig lo multicast

This is set persistently during the polysync_post_install.sh script during installation.

SDF IP address

Lastly, open the SDF Configurator to ensure that the SDF configuration matches the system network configuration. If there is a mismatch detected in the SDF, the Host Wizard will open.

$ polysync-core-sdf-configurator

Distributed Host Setup

This guide will demonstrate configuring a distributed runtime with hosts on a single subnet.

For each host in a multi-host distributed runtime, the following must occur:

  • Designate a PolySync Ethernet interface and IP subnet
  • Determine a unique IP address within the defined subnet
  • PolySync will start PTP services automatically

1. PolySync Ethernet interface

Most machines have at least one Ethernet interface, also known as a Network Interface Card (NIC). To list all available NICs, the developer can use the system command ifconfig:

$ ifconfig

For machines with a single NIC, the Ethernet interface displayed eth0, must be designated as the PolySync Ethernet interface.

2. PolySync IP address

When using a static network, the DDS Discovery feature will search for other hosts through the selected Ethernet interface. If other hosts exist on the network, developers will be sharing the PolySync bus, which means they will see the other host’s data, and vice versa.

Once a PolySync IP address is defined it must exist on an Ethernet interface before any PolySync application is run.

The most common network configuration solutions define a single subnet for all hosts to communicate on. Advanced users may set the subnet mask and perform more sophisticated networking solutions.

When defining hosts on the ‘200’ subnet, it is important to note that all host IP addresses will start with ‘192.168.200’. Each host defined on the subnet will have a unique address. Once an address is selected, use the PolySync Manager to update the PolySync configuration.

$ polysync-core-manager -s 192.168.200.100

The IP address must exist on the host (shown through ifconfig command) before PolySync applications can run.

3. SDF IP address

Open the SDF Configurator to ensure that the SDF configuration matches the system network configuration. If there is a mismatch detected in the SDF, the Host Wizard will open.

$ polysync-core-sdf-configurator`

The last requirement is to run Precision Time Protocol (PTP). Without the PTP service, hosts defined in the PolySync runtime will have differing system times and PolySync data will not be aligned or replayable.

Add New Host

1. Start the SDF Configurator

To add a new host to the SDF, start the SDF Configurator:

$ polysync-core-sdf-configurator

2. Add a new host

Now, select the in the host bar near the top of the application.

Add a new Host

Each host has metadata associated with it that must be set before any nodes can be started.

  • Name
    • Custom name to reference this host
  • Interface address
    • The IP address in the network that needs to be connected to
    • This address must exist on the local host for PolySync nodes to run
  • License file
    • Optional absolute path to a physical license file
    • Typically the license is stored in trusted storage, and this is empty or “(null)”
  • Record & Replay directory
    • Directory to write your logfile sessions to, as well as the directory to search for existing logfile sessions

Move Node To New Host

The move feature in the SDF Configurator retains all existing node information─most importantly the SDF ID─when it’s moved to the new host. This is important since a persistent node ID is required to replay a previously recorded logfile session.

First start the SDF Configurator from the application browser, or the command:

$ polysync-core-sdf-configurator

To move a node there must be more than one host defined in the SDF.

Select any node from the node list, and press the “Move” button on the right-hand side. A new window will populate a list of available hosts, as well as move the node.

SDF Configurator - Moving Nodes

Hooking Up Sensors

Connect USB Webcam

This tutorial will introduce you to the process of connecting a webcam sensor.

When working with sensors it is important to remember that every sensor is slightly different─some require a sensor identifier, or additional Linux configuration requirements, etc. The requirements for specific sensors can be found under Supported Sensors in the help center.

1. Define your node using the PolySync SDF Configurator

The first step towards connecting a sensor is to define your node using the PolySync SDF Configurator . You can either use the Ubuntu icon or enter the following command into a Terminal window to start the SDF Configurator:

$ polysync-core-sdf-configurator

Now you can add a new node. In the bottom left corner of the SDF Configurator window click “Add Sensor Node.” A dialog box will appear with a dropdown list for all of the currently supported sensor types. Choose “video-device” from this list.

After pressing “OK,” all of the parameters for the sensor will be visible. If using multiple sensors, be sure to name the sensor to avoid confusion.

Connecting Cam 1

The above Configurator allows you to manage three main categories of parameters, which help you with everything from physical device settings to file paths:

  • Node Configuration
    • Application settings, such as the file path for the the device driver
  • Sensor Configuration
    • Physical settings of the device, such as XYZ placement on the vehicle
    • Published and logged: Pixel format, image width/height, frames per second
    • Optional extended H264 encoder parameters
  • IO Configuration
    • IO settings for the data, such as video frames per second
    • Source: Pixel format, image width/height, frames per second
    • Source device configuration parameters

Within the SDF Configurator all changes are saved after pressing “Enter.”

Copy the node’s SDF ID. This will be used later to start the node.

2. Enumerate and assess the video device name

When the video device is connected to the Linux machine the Video4Linux drivers will enumerate the device name, similar to /dev/video0.

When the device is initialized during the boot process or connected to the host machine you will see it in /var/log/syslog or you can run dmesg in a terminal before connecting the device to your host machine.

Once the device is connected to the host machine, you can add it in the SDF Configurator IO Configuration panel under “Video Device 0 Name” field.

You can also see all video devices by doing:

$ ls /dev/video*

3. Starting PolySync manager

Next, you’re going to call the PolySync manager daemon and signal it to start the wall clock:

$ polysync-core-manager

Once that has been completed, you can start the node manually by using the -n flag to reference the “Node ID:” in the Node Configuration section of the SDF Configurator:

$ polysync-core-dynamic-driver -n 1

In an “OK” state, this would be the last line the node prints:

2018-04-15 10:57:47.09s DEBUG  [video-device-1688894064433460] - transition to state: OK - timestamp: 1473994633468834

If any errors are being returned, you can use the -u flag to update this node’s supported parameters in the SDF:

$ polysync-core-dynamic-driver -n 1 -u

4. Visualize data using Studio

You are now able to start PolySync Studio in order to visualize the image data. Input the following:

$ polysync-core-studio

Open the video plugin from the button on the plugin loader on the right side of Studio. On the left-hand side of the window you can see the plugin’s control panel. The video device name can be selected from the control panel, and the live input can be seen from the node.

Connecting Cam 2

You can visualize the data with a second node by running the image data viewer example:

$ polysync-image-data-viewer

Conclusion

Congratulations! You have now successfully connected a webcam sensor.

Connect Ethernet Sensor

Ethernet sensors have a static IP address that must be entered in the SDF Configurator to allow PolySync nodes to communicate. Once the sensor’s IP address is known, the ECUs network must be configured to allow PolySync to communicate with the sensor through the Ethernet interface.

The Ethernet interface is the Network Interface Card (NIC) where the sensor is connected to the ECU. On Linux, multiple Ethernet interfaces are commonly shown as eth0, eth1, etc.

1. Listing available interfaces

Determine the Ethernet interface name(s) using the system network tool ifconfig:

$ ifconfig 
eth0      Link encap:Ethernet  HWaddr XX:7b:XX:3b:0d:XX  
          UP BROADCAST MULTICAST  MTU:9000  Metric:1
          RX packets:2893321 errors:0 dropped:0 overruns:0 frame:0
          TX packets:1141728 errors:0 dropped:0 overruns:0 carrier:0
          collisions:0 txqueuelen:1000 
          RX bytes:3150169684 (3.1 GB)  TX bytes:864658858 (864.6 MB)
          Interrupt:20 Memory:e1200000-e1220000 

lo        Link encap:Local Loopback  
          inet addr:127.0.0.1  Mask:255.0.0.0
          inet6 addr: ::1/128 Scope:Host
          UP LOOPBACK RUNNING  MTU:65536  Metric:1
          RX packets:478680 errors:0 dropped:0 overruns:0 frame:0
          TX packets:478680 errors:0 dropped:0 overruns:0 carrier:0
          collisions:0 txqueuelen:0 
          RX bytes:1733767001 (1.7 GB)  TX bytes:1733767001 (1.7 GB)

If there is one Ethernet interface─like the above example─the interface name will likely be eth0. Each of the eth0/eth1/etc entries map directly to a physical Ethernet NIC interface on the machine.

For most machines, you will see at least two entries: eth0 and lo.

2. Set IP address and subnet mask

Once the Ethernet interface name is known, use the ifconfig utility to set a temporary IP address:

$ sudo ifconfig eth0 192.168.2.200 netmask 255.0.0.0

A persistent IP address can be set using the Ubuntu network manager utility.

3. Zenmap / Nmap

Zenmap is an easy to implement front-end wrapper to Nmap. Zenmap scans the specified network for hosts, and can be used to find sensors and ECUs.

Utilities can be downloaded on Ubuntu with apt-get:

$ sudo apt-get install zenmap nmap

4. Searching for Ethernet devices

Start Zenmap to scan the network for other devices.

$ sudo zenmap

You will enter both the IP range they wish to search, as well as the verbosity of the search. To find sensors a “Quick scan” works well.

Target: 192.168.0.1/8
Profile: Quick scan

Searching with Zenmap

Once “Scan” is pressed, it can take up to ten minutes to locate the device depending on the sensor’s IP address and a developer’s host IP address.

One result (the developer’s host) should always be visible. If there are just two returned hosts, then the sensor has been found. If three or more hosts are visible, then narrow the search by removing other sensors or ECU’s from the network.

Next, the PolySync SDF Configurator will need to be updated so that the dynamic driver knows which IP address to connect to. Then set the system IP address by overriding the network manager.

5. Overriding Ubuntu network manager

The Ubuntu network manager defaults Ethernet interfaces to a DHCP mode that expects a dynamic IP address to be provided by a DHCP server (like a router). Most sensors aren’t routers.

It is necessary to switch the mode from DHCP to Manual, which allows you to set a static IP address matching the subnet of the sensor.

Open the network manager, select the appropriate Ethernet interface, and set the IPv4 method from DHCP to Manual. IP address, subnet mask, and gateway are required fields.

Test the new connection with the ECU and sensor by using the system tool ping, and entering the IP address of the sensor.

$ ping <sensors-ip-address>
64 bytes from localhost (<hosts-ip-address>): icmp_seq=1 ttl=64 time=0.027 ms
64 bytes from localhost (<hosts-ip-address>): icmp_seq=2 ttl=64 time=0.033 ms
64 bytes from localhost (<hosts-ip-address>): icmp_seq=3 ttl=64 time=0.027 ms
64 bytes from localhost (<hosts-ip-address>): icmp_seq=4 ttl=64 time=0.038 ms

Conclusion

Congratulations! You have now successfully connected to an Ethernet sensor.

Connect CAN Sensor

This tutorial will walk you through connecting a CAN RADAR sensor using a Delphi ESR 2.5 short range RADAR sensor.

1. CAN Channel Identifiers

Begin by finding the CAN Hardware And Circuit Identifiers of the physical CAN interface.

PolySync Core supports two types of CAN hardware devices, Kvaser and off-the-shelf. Kvaser uses linuxcan libraries to communicate with hardware, while most other off-the-shelf hardware uses SocketCAN libraries for communication.

1.1 Linuxcan

If you are using Kvaser hardware (compatible with linuxcan libraries and drivers), input the following into your terminal window to parse the hardware and circuit IDs:

$ /usr/src/linuxcan/canlib/examples/listChannels

The following example output should be visible:

Found 1 channel.
channel 0 = Kvaser Leaf Light v2, 73-30130-00685-0, (0)21718, 3.3.0.769

In the above example, the hardware identifier is printed near the end, in this case it’s 21718. The circuit identifier is an index for multi-channel CAN hardware interfaces. The (0) preceding the hardware identifier represents the circuit ID.

1.2 SocketCAN

If you are using SocketCAN hardware (compatible with SocketCAN libraries and drivers), you will only need to find the hardware identifier. Available devices can be queried from the system with ifconfig -a, and are typically represented as can0, can1, etc.

The following devices map to the listed values in PolySync:

| SocketCAN Channel | PolySync HW ID | | ————- | ————- | | can0 | 1 | | can1 | 2 | | can2 | 3 | | canN | N+1 |

Now it’s time to configure the sensor node’s IO Parameters in the SDF Configurator .

To start the SDF Configurator from the terminal, input the following:

$ polysync-core-sdf-configurator

2. Define node in the SDF

Next we need to define the CAN node in the SDF using the SDF Configurator. To add a new sensor node press the Add Sensor Node button in the bottom-left of the SDF Configurator.

Select the sensor from the drop-down list and give the node a name.

Once the node has been created in the SDF, the supported parameters for the node are automatically populated by the SDF Configurator with default values.

3. Parse the sensor ID

Now that you’ve properly configured the circuit ID and hardware ID for the node, you will need to obtain the sensor identifier using the PolySync Core dynamic driver interface.

The PolySync dynamic driver will use the ‘-g’ flag to query the sensor identifier. This can only be done after the hardware and circuit IDs are entered into the SDF Configurator.

$ polysync-core-dynamic-driver -n 1 -g

When invoking the dynamic driver, you will use the node ID that defines the sensor node. The node ID can be found in the upper right corner of the node configuration within the SDF Configurator (above).

The information is logged to the file ~/.local/share/polysync/polysync.log, and to standard output by default.

4. Update the SDF

In this final step, you will need to copy the decimal value of the sensor identifier─provided in the output of the dynamic driver node─to the SDF Configurator “Sensor 0 Identifier.”

SDFConfigurator

Now the PolySync Manager can be started.

$ polysync-core-manager -n

The node can also be manually started by referencing the SDF Configurator node ID within the polysync-core-dynamic-driver command. This is done by starting the PolySync manager in wall-clock mode (no arguments) and then running the polysync-core-dynamic-driver command with the -n flag. The -n flag tells the dynamic driver which node ID (found in the SDF Configurator) to reference.

$ polysync-core-manager
$ polysync-core-dynamic-driver -n 1

5. Data visualization

To visualize the sensor data, start PolySync Studio and launch the 3D view or trace plugins.

$ polysync-core-studio

Sensor data is globally available on the PolySync bus to any node that subscribes to the message type(s) output by the dynamic driver sensor node.

Conclusion

Congratulations! You have now successfully connected a CAN RADAR sensor and visualized its output using Studio.

Recording & Replaying Data

Recording Data

PolySync Core is able to record the incoming sensor data, and allows you to replay the data on any other system. Using Studio or command line tools, you can command nodes to start and stop the recording of incoming data.

Both methods use the same underlying logic to record data: the application populates a ps_rnr_msg with the desired state and publishes the message to the bus, which active nodes receive and respond to.

This article details the steps to prepare the system, and then command active runtime nodes to record the incoming sensor data.

1. Recording data with PolySync Core

Each host in a PolySync Core runtime has a unified rnr_logs directory where data is written to. This is defined in the SDF Configurator in the “RnR Logs” directory, and by default is located in ~/.local/share/polysync/rnr_logs.

In a single host system, all data is recorded and written to the same location. Runtime nodes record data on the local host they are executing on in a distributed system.

1.1 Nodes that support record and replay

Custom nodes cannot automatically record and replay data. They must use PolySync APIs to manage logfile data, for example the PolySync-provided nodes.

PolySync-provided nodes each implement file reading and writing routines using the PolySync Logfile API. The nodes also need to listen for the ps_rnr_msg, which uses the Record & Replay API to instruct nodes regarding what files they should open, and when to start/stop recording and replaying data.

Applications like Studio’s replay module implement the Record & Replay API to instruct nodes which directory to record data to, and at what time to start writing data to the disk (now, or in the near future).

1.2 Exporting recorded data

Once a logfile session has been recorded using Studio or the command line tools, the logfile session can be exported from the current system, allowing it to be replayed on any other system.

To replay the recorded data on another system, check out this exporting recorded data article.

2. Commanding record on the command line

There are two different command line tools that allow you to command active runtime nodes to start and stop the recording of data. There’s an example written in C and another written in C++.

This section will show you how to build, run, and effectively use the C rnr_control example.

The example can be cloned from the C repo and built using make:

$ git clone git@github.com:PolySync/PolySync-Core-C-Examples.git
$ cd PolySync-Core-C-Examples/rnr_control
$ sudo apt-get install libglib2.0-dev  # this is required for this example to build, documented in the examples README.md file
$ make

The binary is named polysync-rnr-control-c.

2.1 Starting nodes in hardware mode

Before you can record data on the system, nodes must be running in the hardware mode. See the Runtime section for an article detailing how to start sensor and hardware nodes in hardware mode.

2.2 Commanding active nodes to record data

Before nodes are commanded to record data, it’s best to check the system state to ensure nodes are in a known-good state. Active nodes can be seen in Studio’s system state module.

The polysync-rnr-control-c application uses the -t flag to indicate the desination for the logged data, referred to as a logfile session. The logfile session will contain one plog file for each active sensor at the time of recording. The -w flag tells the application that we want to record data (opposed to replaying data).

To begin recording data to session 1001, use the command:

$ polysync-rnr-control-c -w -t 1001

It’s important to note that the rnr_control application has no knowledge of the nodes on the PolySync bus. It’s simply packaging and publishing a ps_rnr_msg that active nodes subscribe and react to.

2.3 Commanding active nodes to stop recording data

To command nodes to stop recording data, you will need to pass the -q flag. At any time, issue the following command to end the logfile session:

$ polysync-rnr-control-c -q

Nodes will continue to parse sensor data, but they will stop recording and writing data to the logfile session.

Congratulations! You’ve successfully commanded nodes to record data from the command line.

3. Commanding record with studio

Studio has a record module to initiate, monitor, and stop the recording of active nodes. The record module will show up in the logfile management section of Studio when there are one or more nodes running in hardware mode.

All of the tools you need are located in the lower-left corner of Studio.

Studio system state and logfile management recording tools

3.1 Starting nodes in hardware mode

Before you can record data on the system, nodes must be running in hardware mode.

To command nodes into hardware mode, press the Icon button.

For more detailed instructions, see the Runtime section for an article detailing how to start sensor and hardware nodes in hardware mode.

3.2 Start recording data

Before nodes are commanded to record data, it’s best to check the system state to ensure nodes are in a known-good state. Active nodes can be seen in Studio’s system state module.

To begin recording data, press the Icon button. The record module will name the logfile session based on the UTC timestamp of the moment the record button is pressed.

If you cannot see the Icon button, then the system is not operating in hardware mode.

The elapased recording time will show in the logfile management area.

3.3 Command active nodes to stop recording data

When you’ve finished recording your data or scenario, press the Icon button.

Next steps

Congratulations! You’ve successfully commanded nodes to record data from the Studio record module.

The logfile session is now available in Studio’s logfile management area.

The logfile session can be replayed on the local system or exported from Studio, which allows you to import and replay your data on any other PolySync system.

Import and Replay Logfile Data

PolySync Studio allows you to visualize and manage the different kinds of data that sensors have captured in logfile sessions. This article will introduce you to the most commonly used workflow within the PolySync ecosystem.

PolySync Core has the ability to replay prerecorded sensor data stored in a PolySync logfile session.

This article shows you how to import and replay the logfile session 1000 that ships with PolySync Core, but this workflow is used to import and replay any logfile session.

1. Run PolySync manager

Start the PolySync manager as a service before any other PolySync nodes.

$ sudo service polysync-core-manager start

Note that while in the replay state, the system is referencing the replay SDF .

2. Import the logfile session using Studio

Studio provides an interface to export recorded logfile sessions, from a single-host or a distributed system, and import the logfile session to any other PolySync system.

Follow these steps to import a previously exported logfile session.

  1. Start PolySync manager.

    polysync-core-manager
    
  2. Start Studio with the desktop icon, or the command.

    polysync-core-studio
    
  3. Click the icon. Studio landing screen

  4. Right-click within the logfile manager section, or select the icon. Logfile manager area

  5. Navigate to and select the extracted and decompressed logfile session folder 1000 located in /usr/local/polysync/rnr_logs/1000, click the Import button. Logfile manager import window

  6. Allow the logfile session to fully import. Importing In-progress

Studio will begin importing the files to the logfile management section. The import status is shown with a loading bar. A textual status and description of the file transfer is provided in the console debug log messages.

Once the session is visible within the logfile manager, it can be selected for playback by dragging the folder to the playlist and double-clicking the playlist entry.

3. Visualize sample data

Studio visualizes data being published to the PolySync bus , and provides an interface to record and replay PolySync logfile sessions.

Start PolySync Studio from the Ubuntu icon, or from the command line:

$ polysync-core-studio

Studio will open in standby mode, signifying it is ready to replay a logfile session or enter hardware mode. Click the green system state indicator in the bottom-left for more details about the system mode and status.

System state selection

4. Load the logfile session

Open the logfile manager.

Setup

Logfile sessions that are available for replay show up in the logfile manager pane. Select the logfile session that ships with PolySync releases─”Session 1000”─and drag it to the playlist pane on the right.

Click on the arrow to dropdown and list the nodes associated with the logfile session. These are the same nodes defined in the replay SDF for this logfile session.

5. Replay

Before selecting a logfile session for replay, open the 3D View plugin in preparation to visualize data.

To replay a logfile session, double click the session Name in the playlist pane. Each nodes status is shown in the playlist dropdown menu, where you can watch nodes change as they each load an individual plog file.

Ready for Logfile Session Replay

When the selected logfile session has been loaded by all nodes, it will begin playback automatically. The 3D View plugin will begin displaying RADAR and LiDAR data. The Xsens data is visible in the trace plugin, as a ps_platform_motion_msg.

The system state indicator at the bottom-left changes from green to blue to indicate a session is being replayed.

Logfile Session 1000 Playback

If there is an error in loading, the system state indicator at the bottom-left will change to red. Click it for more details, and see solutions to common problems here.

Conclusion

Congratulations! You have successfully replayed and visualized the sample data.

Replaying Data from the Command Line

To replay data, runtime nodes need to be told which logfile session to load. Sensor and hardware nodes subscribe to the ps_rnr_msg to receive commands to enter record or replay modes and know when to start reading or writing the data.

PolySync provides both GUI and command line tools to command runtime nodes to record and replay data.

This article shows how to use the supplied C API rnr_control example application to command nodes to replay an existing logfile session.

1. Clone the repo

The rnr_control code is in the PolySync C Examples GitHub repo.

$ git clone https://github.com/PolySync/PolySync-Core-C-Examples

2. Build the application

The project uses Make to build, and places the binary in the bin/ directory.

$ cd PolySync-Core-C-Examples/rnr_control
$ make
$ make install   # copies to /usr/local/polysync/bin/

The binary is built and is named polysync-rnr-control-c.

3. Record and replay sessions directory

Each host has a defined record and replay (rnr) sessions directory defined in the SDF. The rnr directory is defined and viewed in the SDF configurator, and can also be printed by the license tool:

$ polysync-license-tool
*****************************
*** PolySync License Tool ***
API Build Version: 2.0.9-1486494674

Host ID: '(Ethernet) XXXXXXXXXXXX'
Installation Directory (PSYNC_HOME): '/usr/local/polysync'
System Design File: '/home/${USER}/.local/share/polysync/config/psync.sdf'
Record and Replay Sessions Directory: '/home/${USER}/.local/share/polysync/rnr_logs'
IP Address: '127.0.0.1'
License Server: 'not used'
License file specified in SDF: 'not used'
Current license status: valid

*****************************

The default directory is /usr/local/polysync/rnr_logs. The polysync-rnr-control-c application can only command runtime nodes to replay data contained in this directory.

4. Replay a PolySync logfile

Now that the polysync-rnr-control-c application has been built, we can use it to command nodes to replay PolySync logfiles.

4.1 Starting runtime nodes

Use the manager to start all runtime nodes defined in the system SDF in the replay context.

$ polysync-core-manager -n -w

If the manager is already running, stop it and associated runtime nodes first with the -q.

$ polysync-core-manager -q
$ polysync-core-manager -n -w 

Nodes are now in the standby state, waiting to be told which PolySync logfile to load.

4.2 Commanding replay

The polysync-rnr-control-c application uses the -t flag to indicate which logfile session to replay. All other flags are optional while commanding runtime nodes to replay data.

$ polysync-rnr-control-c -t 1000

This command will replay the default logfile session that ships with PolySync.

It’s important to note that the rnr_control application has no knowledge of the nodes on the PolySync bus. It’s simply packaging and publishing a ps_rnr_msg that active nodes subscribe and react to.

For systems that have a couple dozen nodes, or limited compute resources, it’s nice to provide a loading grace period. Pass in the -s <opt> flag to specify a relative start time (microseconds), or the -S <opt> flag to specify an absolute UTC microsecond start time.

$ polysync-rnr-control-c -t 1000 -s 3000000   # 3 seconds

To stop replay at any time, use the applications -q flag.

$ polysync-rnr-control-c -q

5. Record a PolySync logfile

5.1 Starting runtime nodes

Use the manager to start all runtime nodes defined in the system SDF in the hardware context.

$ polysync-core-manager -n

If the manager is already running stop it, as well as associated runtime nodes, first with the -q flag.

$ polysync-core-manager -q
$ polysync-core-manager -n 

Nodes are now in the standby state, waiting to be told which PolySync logfile to load.

5.2 Commanding replay

The polysync-rnr-control-c application uses the -t flag to indicate which logfile session is being referenced, and the -w flag to indicate that we want to enable record mode.

$ polysync-rnr-control-c -t 2001 -w

This command will tell nodes to begin recording data to the logfile session ~/.local/share/polysync/rnr_logs/2001/.

It’s important to note that the rnr_control application has no knowledge of the nodes on the PolySync bus. It’s simply packaging and publishing a ps_rnr_msg that active nodes subscribe and react to.

To command nodes to stop recording data, use the application’s -q flag.

$ polysync-rnr-control-c -q
Publishing & Subscribing on the bus

Publish and Subscribe Tutorial

In this tutorial, we are going to walk through how to publish a command to the PolySync bus and subscribe to a particular message type.

1. Become a participant on the bus

In order to begin the [term id=“2058”]publish/subscribe communication[/term] process, a PolySync node must first become a valid participant on the bus .

You can do this by calling:

 psync_init()

Once this has occurred, it becomes discoverable by all other nodes in the PolySync runtime, allowing it to publish and subscribe any number of message types.

2. Locate the C++ Code

You can find the C++ Publish Subscribe code as part of our public C++ examples repo here.

PolySync-Core-CPP-Examples/PublishSubscribe
├── Publish
│   ├── CMakeLists.txt
│   └── Publish.cpp
└── Subscribe
    ├── CMakeLists.txt
    └── Subscribe.cpp

2.1 Publish.cpp

#include <PolySyncNode.hpp>
#include <PolySyncDataModel.hpp>

using namespace std;

/**
 * @brief PublisherSubscriberNode class
 *
 *
 */
class PublisherSubscriberNode : public polysync::Node
{

private:
    const string node_name = "polysync-publish-cpp";
    const string platform_motion_msg_name = "ps_platform_motion_msg";

    ps_msg_type _messageType;

public:

    PublisherSubscriberNode()
    {
        setNodeType( PSYNC_NODE_TYPE_API_USER );
        setDomainID( PSYNC_DEFAULT_DOMAIN );
        setSDFID( PSYNC_SDF_ID_INVALID );
        setFlags( PSYNC_INIT_FLAG_STDOUT_LOGGING );
        setNodeName( node_name );
    }

    ~PublisherSubscriberNode()
    {

    }

    void initStateEvent() override
    {
        _messageType = getMessageTypeByName( platform_motion_msg_name );
    }

    void releaseStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void errorStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void fatalStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void warnStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void okStateEvent() override
    {
        // Create a message
        polysync::datamodel::PlatformMotionMessage message( *this );

        // Set the message timestamp (UTC), representing when the data was
        // received or when it originated
        message.setTimestamp( polysync::getTimestamp() );

        message.setLatitude( 45.515289 );

        message.setLongitude( -122.654355 );

        // Set the message header timestamp, representing when the message
        // was published to the PolySync bus
        message.setHeaderTimestamp( polysync::getTimestamp() );

        // Publish the message to the PolySync bus
        message.publish();

        // The ok state is called periodically by the system, sleep to reduce
        // the number of messages sent
        polysync::sleepMicro( 1000000 );
    }
};
int main()
{
    // Create an instance of the PublisherSubscriberNode and connect it to
    // PolySync
    PublisherSubscriberNode publisherNode;

    // When the node has been created, it will cause an initStateEvent to be
    // sent and then proceed into the okState.  connectToPolySync does not
    // return, use Ctrl-C to exit
    publisherNode.connectPolySync();

    return 0;
}

2.2 Publish.cpp explained

Please be aware that both of these headers must be included in C++ applications. The Node API PolySyncNode.hpp creates a node and defines the states within the node state machine. It also allows the node to publish and subscribe to the bus.

The PolySyncDataModel.hpp defines all messages, types, and constants in the PolySync C++ Data Model.

#include <PolySyncNode.hpp>
#include <PolySyncDataModel.hpp>

The following is an inclusive list of the node states that are defined as part of the PolySync node state machine:

  • setConfigurationEvent()
    • Called once at the beginning of the program before any PolySync related tasks, and where command-line arguments should be handled
  • initStateEvent()
    • Called once after the node transitions into the INIT state
  • okStateEvent()
    • Called continuously while in the OK state
  • warnStateEvent()
    • Called continuously while in the WARN state
  • errorStateEvent()
    • Called continuously while in the ERROR state
  • fatalStateEvent()
    • Called once after the node transitions into the FATAL state before the node terminates
  • releaseStateEvent()
    • Called once on exit, triggered by CTRL+C
  • messageEvent()
    • Fires once for each time that a subscribed message is received, access the message data payload

Unless we override the node states, each state does minimal to no operation.

You can then move on to subclass the C++ node object to create a PolySync node.

class PublisherSubscriberNode : public polysync::Node
{ ... }

It is important to remember to set the node name during object construction.

Since the node will be publishing a message to the PolySync bus, it needs to know the string representation for the message type. We can make this known to the node by calling the _message private member. This will hold the integer representation of the message type at runtime, which allows the node to identify and subscribe to the message type.

private:
    const string node_name = "polysync-publish-cpp";
    const string platform_motion_msg_name = "ps_platform_motion_msg";

    ps_msg_type _messageType;

The constructor is then called.

PublisherSubscriberNode()
    {
        setNodeType( PSYNC_NODE_TYPE_API_USER );
        setDomainID( PSYNC_DEFAULT_DOMAIN );
        setSDFID( PSYNC_SDF_ID_INVALID );
        setFlags( NODE_FLAGS_VALUE | PSYNC_INIT_FLAG_STDOUT_LOGGING );
        setNodeName( node_name );
    }

Now, we move on to call the OK state event. It is important to note that this is called once per execution cycle in the node state machine, while the node is in the OK state.

This OK state event will create an instance of the platform motion message, fill it with some sample data, and publish it to the global PolySync bus each time it is called.

You can control the node’s execution speed by allowing it to sleep more or less.

void okStateEvent() override
    {
        ...

        polysync::sleepMicro( 1000000 );
    }

Using the predefined messages in the datamodel, the node creates an instance of the ps_platform_motion_msg using the C++ representation for the message type.

Now, we will call the message timestamp. Each message has some form of a message timestamp, which is used to represent the UTC time signifying when the data was received from a hardware device, PolySync node, or software application.

        polysync::datamodel::PlatformMotionMessage message( *this );
        message.setTimestamp( polysync::getTimestamp() );

At this point, we can allow the node to update the latitude and longitude values for this instance of the platform motion message.

Check the C++ Data Model to see all data fields that exist and can be set in this message type.

        message.setLatitude( 45.515289 );

        message.setLongitude( -122.654355 );

We will now move on to setting the message header timestamp. This represents the UTC time when the message was published to the PolySync bus. The publishing node is always responsible for setting this directly before publishing the message to the PolySync bus.

        message.setHeaderTimestamp( polysync::getTimestamp() );

        message.publish();

Now we will connect the node to PolySync. The main entry point creates an instance of the custom publisher/subscriber node and immediately connects to PolySync, placing the node in the setConfigurationEvent and then the initStateEvent.

All non-PolySync related initialization should happen in the nodes setConfigurationEvent.

The node will exit the state machine execution loop once it receives the CTRL-C signal interrupt.

int main()
{
    PublisherSubscriberNode publisherNode;

    publisherNode.connectPolySync();

    return 0;
}

2.3 Subscribe.cpp

#include <iostream>
#include <PolySyncNode.hpp>
#include <PolySyncDataModel.hpp>

using namespace std;

/**
 * @brief Node flags to be OR'd with driver/interface flags.
 */
#ifndef NODE_FLAGS_VALUE
#define NODE_FLAGS_VALUE (0)
#endif
class PublisherSubscriberNode : public polysync::Node
{
private:
    const string node_name = "polysync-subscribe-cpp";
    const string platform_motion_msg_name = "ps_platform_motion_msg";

    ps_msg_type _messageType;

public:

    PublisherSubscriberNode()
    {
        setNodeType( PSYNC_NODE_TYPE_API_USER );
        setDomainID( PSYNC_DEFAULT_DOMAIN );
        setSDFID( PSYNC_SDF_ID_INVALID );
        setFlags( NODE_FLAGS_VALUE | PSYNC_INIT_FLAG_STDOUT_LOGGING );
        setNodeName( node_name );
    }

    ~PublisherSubscriberNode()
    {

    }

    void initStateEvent() override
    {
        _messageType = getMessageTypeByName( platform_motion_msg_name );

        // Register as a listener for the platform motion message
        registerListener( _messageType );
    }

    void releaseStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void errorStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void fatalStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void warnStateEvent() override
    {
        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro( 10000 );
    }

    void okStateEvent() override
    {
        // The ok state is called periodically by the system
        polysync::sleepMicro( 10000 );
    }

    void messageEvent( std::shared_ptr< polysync::Message > message ) override
    {
        using namespace polysync::datamodel;
        if( std::shared_ptr < polysync::datamodel::PlatformMotionMessage > platformMotionMsg =
                getSubclass< PlatformMotionMessage >( message ) )
        {  
            //platformMotionMsg->print();

            // or...

            std::cout << "Latitude: " << platformMotionMsg->getLatitude() << std::endl;

            std::cout << "Longitude: " << platformMotionMsg->getLongitude() << std::endl;

        }
    }

};

int main()
{
    // Create an instance of the PublisherSubscriberNode and connect it to
    // PolySync.
    PublisherSubscriberNode subscriberNode;
    // When the node has been created, it will cause an initStateEvent to be
    // sent and then proceed into the okState.  connectToPolySync does not
    // return, use Ctrl-C to exit.
    subscriberNode.connectPolySync();

    return 0;
}

2.4 Subscribe.cpp explained

The subscriber is largely similar to the publisher, and repeated variables and identical functions will be skipped here.

The message subscriber nodes should always query the runtime for the message type integer values, based on the string representation of the message type.

Once the message type is known, the node can resister a listener. For every instance this message type is seen on the PolySync bus, this node’s message event will be executed once.

    void initStateEvent() override
    {
        _messageType = getMessageTypeByName( platform_motion_msg_name );

        registerListener( _messageType );
    }

The message event executes asynchronously in its own thread.

We will be using the defined C++ class template. This acts to promote the incoming base message to the appropriate type─the string representation of the message.

    void messageEvent( std::shared_ptr< polysync::Message > message ) override
    {
        using namespace polysync::datamodel;
        if( std::shared_ptr < polysync::datamodel::PlatformMotionMessage > platformMotionMsg =
                getSubclass< PlatformMotionMessage >( message ) )
        {  
            ...
        }
    }

Each message in the C++ API has a built-in print() method for quick and easy debugging. It will print each message field to std::out, or a provided ostream. Alternatively─and more commonly─you can access the data using the get*() functions.

It’s best to do as little processing in the nodes message event as possible. If the node needs to perform any intense processing it should be moved to another thread by queuing the incoming messages.

            //platformMotionMsg->print();

            // or...

            std::cout << "Latitude: " << platformMotionMsg->getLatitude() << std::endl;

            std::cout << "Longitude: " << platformMotionMsg->getLongitude() << std::endl;

3. Running the nodes

After building the two nodes using cmake and make, you can run them using two terminal windows.

3.1 Building the nodes

If you want to see the build dependencies of a PolySync node, you can check out the CMakeLists.txt file, and the included PSYNC_HOME/BuildResources.cmake file.

$ cd PolySync-Core-CPP-Examples/PublishSubscribe/Publish
$ mkdir build && cd build
$ cmake ..
$ make

Next, you’re going to build the subscriber node in a second terminal window.

$ cd PolySync-Core-CPP-Examples/PublishSubcribe/Subscribe
$ mkdir build && cd build
$ cmake ..
$ make

4. Running the publisher node

It is time to start the publisher node. This will begin publishing the platform motion message to the global PolySync bus, with latitude and longitude set to our static values.

$ ./polysync-publisher-cpp
2016-09-17 14:35:33.09s DEBUG  [polysync-publish-cpp-] - build version 2.0.6-1468018065
2016-09-17 14:35:34.09s DEBUG  [polysync-publish-cpp-281475340021795] - created participant - GUID: 0x0001000015A7B023 d(281475340021795)
2016-09-17 14:35:34.09s DEBUG  [polysync-publish-cpp-281475340021795] - message types visible to this node: 46
2016-09-17 14:35:34.09s DEBUG  [polysync-publish-cpp-281475340021795] - transition to state: INIT - timestamp: 1474148134509200
2016-09-17 14:35:34.09s DEBUG  [polysync-publish-cpp-281475340021795] - transition to state: OK - timestamp: 1474148134519985

5. Running the subscriber node

Now you will run the subscriber node. This connects to PolySync and will begin receiving data as soon as it enters the “OK” state.

$ ./polysync-subscriber-cpp
2016-09-17 14:35:36.09s DEBUG  [polysync-subscribe-cpp-] - build version 2.0.6-1468018065
2016-09-17 14:35:37.09s DEBUG  [polysync-subscribe-cpp-281476857950370] - created participant - GUID: 0x00010000702170A2 d(281476857950370)
2016-09-17 14:35:37.09s DEBUG  [polysync-subscribe-cpp-281476857950370] - message types visible to this node: 46
2016-09-17 14:35:37.09s DEBUG  [polysync-subscribe-cpp-281476857950370] - transition to state: INIT - timestamp: 1474148137993624
2016-09-17 14:35:37.09s DEBUG  [polysync-subscribe-cpp-281476857950370] - transition to state: OK - timestamp: 1474148137996558
Latitude: 45.5153
Longitude: -122.654
Latitude: 45.5153
Longitude: -122.654

6. Visualize the data

The latitude and longitude data can be visualized in Studio’s Trace plugin, by selecting the ps_platform_motion_msg.

To run Studio, enter the following:

$ polysync-core-studio

Conclusion

Congratulations! As you’ve worked through this tutorial, you’ve been introduced to the core concepts of PolySync communication through a message publisher/subscriber model, the basis for creating C++ nodes to access data from the global PolySync bus, and building the node against the PolySync APIs.

Building a Node

1. Scaffolding

The scaffolding generator makes it easy to roll out a functioning C++ software node in PolySync.

To generate a new node, open a new terminal and type:

$ polysync-generate node

This will create a project called MyNode, which can be run with:

$ ./MyNode/build/MyNode

By default MyNode subscribes to and prints PolySync diagnostic messages - DiagnosticTraceMessage.

2. Command line options

Print the command line options with the help flag. You can specify the node name,output directory and which messages to subscribe to with a few flags.

$ polysync-generate node -h
usage: polysync-generate node [-h] [-n NAME] [-o DIR]
                              [-s MESSAGE [MESSAGE ...]] [-l]

optional arguments:
  -h, --help            show this help message and exit
  -n NAME, --name NAME  name of the generated PolySync node
  -o DIR, --output DIR  path in which to place generated files
  -s MESSAGE [MESSAGE ...], --subscribe MESSAGE [MESSAGE ...]
                        messages to which to subscribe
  -l, --list            list valid PolySync messages

2.1 About the code

MyNode contains all of the necessary methods every PolySync C++ node should have. This is the working directory:

MyNode
├── include
│   └── MyNode.hpp
├── CMakeLists.txt
└── src
    ├── MyNode.cpp
    └── main.cpp

In main.cpp, you can see how PolySync nodes are initialized.

The connectPolySync( ) is a blocking operation, and places the node in the node state machine. If there is a valid license, the node leaves the AUTH state and enters the INIT state, calling the initStateEvent( ) in MyNode.cpp.

2.2 main.cpp

#include < iostream >

#include < GetOpt.hpp >
#include < PolySyncCore.hpp >
#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

#include "MyNode.hpp"


using namespace std;


int main( int argc, char * argv[] )
{
    MyNode node;

    node.setNodeName( MyNode::NODE_NAME );

    node.setCommandLineOptions( { argc, argv } );

    node.connectPolySync();

    return 0;
}

2.3 MyNode.hpp

#ifndef MYNODE_HPP
#define MYNODE_HPP


class MyNode : public polysync::Node
{

public:
    virtual void messageEvent( std::shared_ptr< polysync::Message > message );


    virtual void initStateEvent();


    virtual void setConfigurationEvent( const GetOpt & commandLineArgs );


    void okStateEvent();


    virtual void fatalStateEvent();


    virtual void errorStateEvent();


    virtual void warnStateEvent();


    virtual void releaseStateEvent();


public:

    static constexpr auto NODE_NAME = "MyNode";


private:

    ps_msg_type _messageTypeDiagnosticTraceMsg;

};

#endif // MYNODE_HPP

2.4 MyNode.cpp

#include <iostream>

#include < GetOpt.hpp >
#include < PolySyncCore.hpp >
#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

#include "MyNode.hpp"


using namespace std;


void MyNode::messageEvent( std::shared_ptr< polysync::Message > message )
{
    using namespace polysync::datamodel;

    if( auto diagnosticTraceMsg = getSubclass< DiagnosticTraceMessage >( message ) )
    {
        diagnosticTraceMsg->print();
    }
}


void MyNode::initStateEvent()
{
     try
     {
         _messageTypeDiagnosticTraceMsg = getMessageTypeByName( "ps_diagnostic_trace_msg" );
         registerListener( _messageTypeDiagnosticTraceMsg );
     }
     catch (polysync::DTCException e)
     {
         polySyncLogDebug( "ps_diagnostic_trace_msg - " + e.getDtcDescription() );
         activateFault( e.getDtc(), NODE_STATE_FATAL );
     }
}


void MyNode::setConfigurationEvent( const GetOpt & commandLineArgs )
{
    commandLineArgs.printAll();
}


void MyNode::okStateEvent()
{
    // called continuously while in OK state
}


void MyNode::fatalStateEvent()
{
    // called once upon entering FATAL state
}


void MyNode::errorStateEvent()
{
    // called continuously while in ERROR state
}


void MyNode::warnStateEvent()
{
    // called continuously while in WARN state
}


void MyNode::releaseStateEvent()
{
    // called at end of node life
}

2. Building software node

$ cd MyNode 
$ mkdir build && cd build
$ cmake ..
$ make
$ ./MyNode

The scaffolding generator is a great tool to get you developing custom nodes in PolySync quickly.

Publishing Multiple Message Types (Adv.)

This advanced tutorial covers how to populate and publish data from a single node into a PolySync message on the bus, enable multiple nodes to subscribe to messages to access data asynchronously, and visualize all data being published to the PolySync bus.

Overview

The example program polysync-data-generator-cpp is included in the PolySync installation. It is designed to populate PolySync messages with sample data and publish those messages to the global, shared bus.

The sample data comes in multiple types, each with its own associated high-level PolySync message:

  • Radar Targets: ps_radar_targets_msg
  • Classified Objects: ps_objects_msg
  • LiDAR Points: ps_lidar_points_msg

PolySync messages are defined in multiple data model modules:

1. Running the data generator

Begin by starting the PolySync manager. Open a new terminal and run:

$ sudo service polysync-core-manager start

1.2 Generate data

You can find the C++ Data Generation code as part of our public C++ examples repo here.

To build the C++ application you will use the tools cmake and make. The runtime dependencies are detailed in the local README.md file.

$ git clone git@github.com:PolySync/PolySync-Core-CPP-Examples.git
$ cd PolySync-Core-CPP-Examples/DataGenerator
$ sudo apt-get install libglib2.0-dev  # this is required for this example to build, documented in the examples README.md file
$ mkdir build && cd build
$ cmake ..
$ make

Now the polysync-data-generator-cpp node is built, and can be started with the command:

$ ./polysync-data-generator-cpp

This is an overview of the source files for the example:

PolySync-Core-CPP-Examples/DataGenerator/
├── CMakeLists.txt
├── DataGenerator.cpp
├── LidarPointGenerator.cpp
├── LidarPointGenerator.hpp
├── ObjectGenerator.cpp
├── ObjectGenerator.hpp
├── RadarTargetGenerator.cpp
├── README.md
└── RadarTargetGenerator.hpp

1.2.1 DataGenerator.cpp

This is where the PolySync node is defined, and where you can access the main entry point for this node/application.

#include < iostream >

#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

#include "LidarPointGenerator.hpp"
#include "RadarTargetGenerator.hpp"
#include "ObjectGenerator.hpp"

using namespace polysync::datamodel;

class DataGenerator : public polysync::Node
{
protected:
    virtual void okStateEvent()
    {
        _lidarPointGenerator->updatePoints();
        _lidarPointGenerator->publishPoints();

        _radarTargetGenerator->updateTargets();
        _radarTargetGenerator->publishTargets();

        _objectGenerator->updateObjects();
        _objectGenerator->publishObjects();

        polysync::sleepMicro( _updateInterval );
    }
    virtual void initStateEvent()
    {
        _lidarPointGenerator =
                std::unique_ptr< LidarPointGenerator >{
                    new LidarPointGenerator( *this ) };

        _radarTargetGenerator =
                std::unique_ptr< RadarTargetGenerator >{
                    new RadarTargetGenerator( *this ) };

        _objectGenerator =
                std::unique_ptr< ObjectGenerator >{
                    new ObjectGenerator( *this ) };
    }

private:
    ps_timestamp _updateInterval{ 50000 };
    std::unique_ptr< LidarPointGenerator > _lidarPointGenerator;
    std::unique_ptr< RadarTargetGenerator > _radarTargetGenerator;
    std::unique_ptr< ObjectGenerator > _objectGenerator;
};
int main()
{
    DataGenerator dataGenerator;

    dataGenerator.connectPolySync();

    return 0;
}

1.2.2 DataGenerator.cpp explained

Now let’s break down the DataGenerator.cpp code.

The line below defines the PolySync node and each of the protected event methods in the node state machine. Each of the states do minimal or no operation unless they are overridden.

#include < PolySyncNode.hpp >

The C++ data model defines all types and messages in the PolySync runtime. This must be included in all C++ node applications.

#include < PolySyncDataModel.hpp >

The following headers are specific to this application and define the data “generators.” They are responsible for creating, populating, and updating the respective message types with data.

#include "LidarPointGenerator.hpp"
#include "RadarTargetGenerator.hpp"
#include "ObjectGenerator.hpp"

All of our PolySync C++ applications must then subclass the PolySync node object in order to connect to the PolySync bus and publish/subscribe messages.

class DataGenerator : public polysync::Node

The following is called continuously while in the node’s OK state, and is where almost all of the action happens for the node.

virtual void okStateEvent()

For each execution loop, each of the three primitive data types will be updated in a new message instance. These are then published to the bus.

_lidarPointGenerator->updatePoints();
_lidarPointGenerator->publishPoints();

_radarTargetGenerator->updateTargets();
_radarTargetGenerator->publishTargets();

_objectGenerator->updateObjects();
_objectGenerator->publishObjects();

This is called once after the node transitions to the INIT state. The initStateEvent is typically where a node creates messages and initializes any other resources that require a PolySync node reference.

virtual void initStateEvent()

This creates a reference to the new LiDAR point generator instance.

_lidarPointGenerator = std::unique_ptr< LidarPointGenerator >{ new LidarPointGenerator( *this ) };

The following is how you can control the execution speed of the state machine. The _updateInterval is called at the end of each event, whether it has been overridden or not.

ps_timestamp _updateInterval{ 50000 };

This creates a reference for each of your data generator objects defined in their unique header and source files.

std::unique_ptr< LidarPointGenerator > _lidarPointGenerator;
std::unique_ptr< RadarTargetGenerator > _radarTargetGenerator;
std::unique_ptr< ObjectGenerator > _objectGenerator;

This creates an instance of your DataGenerator object. The second command is a blocking operation, and places the node in the node state machine. If there is a valid license, the node leaves the AUTH state and enters the INIT state, calling the initStateEvent.

DataGenerator dataGenerator;
dataGenerator.connectPolySync();

1.3 Generate LiDAR points

Th LiDAR point generator defines the class that is used to create, initialize, populate, and publish LiDAR message on PolySync. This is reimplemented in RadarTargetGenerator.cpp and ObjectGenerator.cpp for a RADAR generator class and an object’s generator class.

#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >


class LidarPointGenerator
{
public:
    LidarPointGenerator( polysync::Node & );


    void updatePoints();


    void publishPoints();


    void initializeMessage();

private:

    polysync::datamodel::LidarPointsMessage _message;

    float _relativeTime{ 0.0 };
    const float _gridScale{ 10.0 };
    const ulong _gridSideLength{ 100 };
    const ulong _sensorID{ 11 };
    const ulong _numberOfPoints{ 10000 };
    const float _sineFrequency{ 4.0 };
};

1.3.1 LidarPointsGenerator.hpp explained

Now let’s break down the LidarPointsGenerator.hpp code.

#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

The three primitive data types (LiDAR, RADAR, objects) each create a class with a basic PolySync node constructor and three functions to initialize, update, and publish data to the bus. The LiDAR point generator class is then called.

class LidarPointGenerator
{ ... };

This class must be constructed with a valid node reference, which happens in the initStateEvent defined in the DataGenerator.cpp file.

public:
    LidarPointGenerator( polysync::Node & );
};

This will be used to populate the LidarPointsMessage with a valid sensor descriptor─describing this node as a publisher on the bus─as well as set the initial header, and message start/end timestamps.

Once the message is initialized, you will use function below to call updatePoints.

public:
    ...
    void initializeMessage();
};

For each message published to the bus, the node needs to update each of the valid fields within the message. updatePoints iterates over a vector of PolySync LidarPoint and updates each LiDAR points:

  • Intensity
  • x, y and z position

Each point in the vector is updated to represent a sine wave.

public:
    ...
    void updatePoints();
};

After you’re done setting the header timestamp, the following will publish the LidarPointsMessage with an updated vector of points to the PolySync bus.

public:
    ...
    void publishPoints();
};

Next, you will call this high-level message used to store LiDAR points.

private:

    polysync::datamodel::LidarPointsMessage _message;
};

The variables are then used to calculate the sine wave and determine the number of LiDAR points required to represent it.

private:
    ...
    float _relativeTime{ 0.0 };
    const float _gridScale{ 10.0 };
    const ulong _gridSideLength{ 100 };
    const ulong _numberOfPoints{ 10000 };
    const float _sineFrequency{ 4.0 };
};

1.3.2 LidarPointsGenerator.cpp

#include "LidarPointGenerator.hpp"

using namespace std;
using namespace polysync::datamodel;

LidarPointGenerator::LidarPointGenerator( polysync::Node & node )
    :
    _message( node ),
    _numberOfPoints( _gridSideLength * _gridSideLength )
{
    initializeMessage();
}

void LidarPointGenerator::initializeMessage()
{
    polysync::datamodel::SensorDescriptor descriptor;

    descriptor.setTransformParentId( PSYNC_COORDINATE_FRAME_LOCAL );
    descriptor.setType( PSYNC_SENSOR_KIND_NOT_AVAILABLE );
    _message.setSensorDescriptor( descriptor );

    auto time = polysync::getTimestamp();
    _message.setHeaderTimestamp( time );
    _message.setStartTimestamp( time );
    _message.setEndTimestamp( time );

    updatePoints();
}

void LidarPointGenerator::updatePoints()
{
    auto time = polysync::getTimestamp();
    auto timeDelta = time - _message.getStartTimestamp();
    auto timeDeltaSeconds = static_cast< float >( timeDelta ) / 1000000.0;

    _relativeTime += timeDeltaSeconds;

    _message.setStartTimestamp( time );
    _message.setEndTimestamp( time );

    std::vector< LidarPoint > outputPoints;
    outputPoints.reserve( _numberOfPoints );

    for( auto pointNum = 0U; pointNum < _numberOfPoints; ++pointNum )
    {
        polysync::datamodel::LidarPoint point;
        point.setIntensity( 255 );

        auto x = pointNum % 100;
        auto y = pointNum / 100;

        float u = static_cast< float >( x )/ 100.0;
        float v = static_cast< float >( y ) / 100.0;

        // center u/v at origin
        u = ( u * 2.0 ) - 1.0;
        v = ( v * 2.0 ) - 1.0;

        float w = sin( ( u * _sineFrequency ) + _relativeTime )
                * cos( ( v * _sineFrequency ) + _relativeTime )
                * 0.5;

        point.setPosition( { u * 10, v * 10, w * 10 } );
        outputPoints.emplace_back( point );
    }

    _message.setPoints( outputPoints );
}

void LidarPointGenerator::publishPoints()
{
    _message.setHeaderTimestamp( polysync::getTimestamp() );
    _message.publish();
}

1.3.3 LidarPointsGenerator.cpp explained

Now let’s break down the LidarPointsGenerator.cpp code.

During the object’s construction, the LiDAR message is initialized with the node reference provided from the DataGenerator.cpp initStateEvent function call. Once you initialize the message here, the message fields can be set to the desired values.

LidarPointGenerator::LidarPointGenerator( polysync::Node & node )
    :
    _message( node ),
    _numberOfPoints( _gridSideLength * _gridSideLength )
{
    initializeMessage();
}

The sensor descriptor is used by the subscribing node to determine which node the message originated from. It is not mandatory, but we highly recommend it as a means to populate the sensor descriptor for each message instance.

The TransformParentId represents the active coordinate frame identifier. In this case, you are setting the active coordinate frame to the local frame, which means no transform corrections are needed for outgoing data.

Messages should be initialized with the current UTC timestamp, multiple timestamps fields exist.

void LidarPointGenerator::initializeMessage()
{
    polysync::datamodel::SensorDescriptor descriptor;

    descriptor.setTransformParentId( PSYNC_COORDINATE_FRAME_LOCAL );
    descriptor.setType( PSYNC_SENSOR_KIND_NOT_AVAILABLE );
    _message.setSensorDescriptor( descriptor );

    auto time = polysync::getTimestamp();
    _message.setHeaderTimestamp( time );
    _message.setStartTimestamp( time );
    _message.setEndTimestamp( time );


    updatePoints();
}

This function is responsible for setting each point in the LiDAR points message, stored as a vector of PolySync LidarPoint. You should be aware that the namespace is polysync::data model.

Timestamp fields are updated for each iteration that the message is updated and subsequently published by publishPoints(). PolySync timestamps can be referenced globally across the PolySync bus, and they are used here both to calculate the sine wave, as well as in the message timestamp fields.

void LidarPointGenerator::updatePoints()
{
    auto time = polysync::getTimestamp();

    auto timeDelta = time - _message.getStartTimestamp();

    auto timeDeltaSeconds = static_cast< float >( timeDelta ) / 1000000.0;

    _relativeTime += timeDeltaSeconds;

    _message.setStartTimestamp( time );
    _message.setEndTimestamp( time );
}

Each point is iterated over to set the intensity, and x/y/z position values before it’s emplaced within the vector. The following for loop will be used to calculate the position of each point to represent a sine wave.

Once the vector is filled the setPoints member function is used to copy the local vector into the message.

    std::vector< LidarPoint > outputPoints;
    outputPoints.reserve( _numberOfPoints );

    for( auto pointNum = 0U; pointNum < _numberOfPoints; ++pointNum )
    {
        polysync::datamodel::LidarPoint point;
        point.setIntensity( 255 );

        auto x = pointNum % 100;
        auto y = pointNum / 100;

        float u = static_cast< float >( x )/ 100.0;
        float v = static_cast< float >( y ) / 100.0;

        // center u/v at origin
        u = ( u * 2.0 ) - 1.0;
        v = ( v * 2.0 ) - 1.0;

        float w = sin( ( u * _sineFrequency ) + _relativeTime )
                * cos( ( v * _sineFrequency ) + _relativeTime )
                * 0.5;

        point.setPosition( { u * 10, v * 10, w * 10 } );
        outputPoints.emplace_back( point );
    }

    _message.setPoints( outputPoints );
}

Now the header timestamp will be set─immediately before publishing─to represent when the UTC timestamp of this message will be published to the PolySync bus.

void LidarPointGenerator::publishPoints()
{
    _message.setHeaderTimestamp( polysync::getTimestamp() );
    _message.publish();
}

2. Visualize data

Once a node─in this case polysync-data-generator-cpp─is publishing data to the PolySync bus, any other application can access the data by subscribing to the PolySync message type(s).

PolySync provides two applications that are already set up to subscribe to the message types published by this node, and visualize the data in multiple ways:

  • PolySync Studio
    • A Qt based application that has multiple plugins providing different ways to view the data on the bus.
  • Viewer Lite
    • An OpenGL 2D visualizer application that allows you to see primitive data types being transmitted using the PolySync bus.

2.1 Viewer Lite

To run PolySync Viewer Lite input the following command:

$ polysync-viewer-lite

DataGen

Your sample node should display the following:

  • A LiDAR “point cloud” as the the block in the center
  • Two “identified objects” as the rectangles on the upper right side of the point cloud. These are similar to those that you might get from an object identification sensor
  • Two circles representing RADAR targets on the upper left side of the point cloud

PolySync Viewer Lite does not allow for much distinction in the point cloud, but updates to the object data and RADAR pings that are immediately obvious.

2.2 Studio

In this instance, the 3D View plugin is the easiest way to view the provided data.

Leave the PolySync Viewer Lite application running, and either double-click the PolySync Studio icon on your desktop or execute the following command in a new terminal:

$ polysync-core-studio

When Studio opens, you will be able to visualize the data that the PolySync Data Generator is creating, select the “3D View” button on the right. With that item selected, PolySync Studio will display something similar to the image:

DataGen

Conclusion

Congratulations! You have now successfully populated and published data from a single node into a PolySync message on the bus, enabled multiple nodes to subscribe to messages to access data asynchronously, and visualized all data being published to the PolySync bus.

Workflow integrations

ROS Bridge

This bidirectional bridge enables PolySync message to be transcribed to ROS topics, and ROS topics to be transcribed to PolySync messages at runtime. PolySync provides a toolchain to build and run a ROS bridge node, which acts as a bridge between the PolySync bus and the ROS bus.

This bridge enables synchronized sensor data from PolySync Core to flow into existing ROS applications. Allowing you to take advantage of PolySync Core’s massive suite of supported sensors, providing submillisecond timing fidelity and no data-loss, while using existing ROS applications and software APIs to create autonomous vehicle algorithms.

Here’s an example of LiDAR, RADAR, and object data visualized in RVIS, streaming from the PolySync bus via the bridge.

ROS bridge example

The ROS bridge node can be generated against any PolySync Core data model using the PolySync provided pdm-gen tool. Once the files are generated the ROS bridge node can be spun up to subscribe to data from the PolySync Core bus, which the ROS bridge node begins transcribing and republishing to the ROS bus.

1. Building the ROS bridge

The pdm-gen tool takes in IDL files as input arguments. The IDL files define C-style structures which make a PolySync high level message, for example the ps_objects_msg and ps_lidar_points_msg.

After sourcing the ROS environment, use the pdm-gen tool and pass in the IDL files containing the message types that you want transcribed to ROS topics.

$ echo $PSYNC_HOME
/usr/local/polysync
$ cd /usr/local/polysync
$ source /opt/ros/jade/setup.bash
$ pdm-gen -r modules/dtc/dtc.idl modules/sensor/sensor.idl modules/navigation/navigation.idl modules/control/control.idl <path-to-other-idl-files>

The command above will generate a ROS topic for every message type defined in the PolySync data model. Create and supply additional IDL files to generate ROS topics for your custom-defined message types.

The pdm-gen tool creates all ROS resources (scripts, headers, libraries) along with a custom ROS topic for each PolySync message that’s defined within the data model IDL files.

The output directory is ./pdm, and the generated ROS resources are in ./pdm/ros_bridge/. You do not need to manually copy files out of the pdm/ directory.

Notable files:

  • pdm/ros_bridge/lib/libps_ros_bridge_library.so
  • pdm/ros_bridge/include/RosMessageCopy.h
  • pdm/ros_bridge/share/polysync_ros_bridge/BridgeNode

2. Running the ROS bridge

Print help:

rosrun polysync_ros_bridge BridgeNode -h
Node Type is a required option all others are optional
Options:
Flag:     Description:                Default:
-t        Node Type
-o        PolySync Ok State Timing    1
-n        Node Name Prefix            ROS_Bridge
-w        Number of ROS Threads       1
-q        ROS Queue Size              1
-p        ROS Topic Prefix            polysync
-m        Print available messages
-h        Prints help

A node type is equivalent to the message type name, for example ps_gps_msg or ps_lidar_points_msg.

2.1 Enumerating available message types

The bridge node enumerates the supported messages that can be subscribed to on the PolySync side, and be translated to a complimentary ROS topic.

$ rosrun polysync_ros_bridge BridgeNode -m
Available bridge messages are:
ps_platform_cabin_report_msg
ps_rnr_msg
ps_file_transfer_msg
ps_imu_accuracy_msg
ps_parameters_msg
ps_event_msg
ps_platform_tire_pressure_report_msg
ps_platform_motion_msg
ps_radar_targets_msg
ps_image_data_msg
ps_platform_wheel_speed_report_msg
ps_file_transfer_ext_msg
ps_objects_msg
ps_platform_obd_msg
ps_imu_msg
ps_platform_suspension_report_msg
ps_platform_surround_report_msg
ps_zones_msg
ps_gps_msg
ps_file_msg
ps_byte_array_msg
ps_response_msg
ps_lane_model_msg
ps_platform_steering_report_msg
ps_platform_throttle_report_msg
ps_platform_brake_information_msg
ps_traffic_sign_msg
ps_lidar_points_msg
ps_diagnostic_trace_msg
ps_can_frame_msg
ps_platform_steering_command_msg
ps_platform_brake_report_msg
ps_gps_accuracy_msg
ps_platform_turn_signal_command_msg
ps_sdf_state_msg
ps_file_ext_msg
ps_platform_brake_command_msg
ps_command_msg
ps_manager_status_msg
ps_platform_throttle_command_msg
ps_platform_gear_command_msg
ps_platform_cabin_ext_report_msg
ps_platform_gear_report_msg
ps_rnr_sessions_msg
ps_platform_control_msg

2.2 Starting the bridge node

For each message type that needs to be transcribed from the PolySync bus to the ROS bus, pass in the -t flag and the message names.

$ source /usr/local/polysync/pdm/ros_bridge/setup.bash
$ roscore
$ polysync-core-manager
$ rosrun polysync_ros_bridge BridgeNode -t ps_objects_msg ps_imu_msg ps_gps_msg

3. Using the ROS bridge

To see an example of how to utilize the ROS bridge, see this example from the PolySync Core C++ GitHub repo.

MATLAB to Export - BETA

The BETA version of the PolySync-MATLAB/Simulink support provides a workflow to export time synchronized data from PolySync, and import the data into MATLAB/Simulink for application and algorithm development.

The BETA version supports these six message types:

  • ps_lane_model_msg
  • ps_lidar_points_msg
  • ps_objects_msg
  • ps_platform_motion_msg
  • ps_radar_targets_msg
  • ps_traffic_sign_msg

This workflow was built as a proof-of-concept between The MathWorks and PolySync. Full-featured support is under joint-development and is expected to be available near Q2 2017.

The full version will support all message types, and will have a streaming interface rather than the export/import process.

1. Workflow

Here’s a high-level overview of the workflow for accessing time-synchronized data in MATLAB:

  1. Build and run the CSV export node
  2. Replay data to export the supported high-level message types to CSV
  3. Use provided MATLAB tools and examples to import CSV and begin autonomous vehicle application development

1.1 CSV export node

PolySync CSV export node─polysync-csv-export─subscribes to the appropriate message types at runtime, and is responsible for writing a CSV file containing all data from each message received.

The source code can be found in the GitHub example [here]().

1.1.1 Building the node

The node comes precompiled with the PolySync release package, or follow these steps to clone the C++ examples repo and build the node:

$ git clone https://github.com/PolySync/PolySync-Core-CPP-Examples
$ cd PolySync-Core-CPP-Examples/CSVExporter
$ mkdir build && cd build
$ cmake .. && make
1.1.1 Running the node

The node can be run anywhere on the system. It takes in the message types to convert to CSV as command line arguments.

$ polysync-csv-export -t ps_objects_msg ps_lane_model_msg ps_radar_targets_msg

Each message type will create its own CSV file in the same directory where the node is run.

To see all available command line options, pass in the help -h flag.

1.2 Replay data on PolySync

Once the CSV export node is running, start nodes in the hardware context, or replay a logfile session to begin publishing data to the PolySync bus via the polysync-core-dynamic-driver nodes.

The CSV export node will automatically receive the messages and begin exporting the data to CSV.

To stop the polysync-csv-node use CTRL+C, the node will disconnect from PolySync and stop receiving messages. The node will continue to write all data currently in the buffer.

1.3 Import data in MATLAB

Download the MATLAB import resources directly from here.

The sample CSV data can be downloaded separately from our Dropbox.

There are two MATLAB project folders included. The +polysync/ and +bev/ folders can be opened using MATLAB.

  • +polysync/
    • Contains the MATLAB data structures, derived from the original PolySync message types
    • Contains the read_polysync_csv_* routines and associated resources to read the PolySync CSV data
  • +bev/
    • Contains the resources to plot detection’s
1.3.1 Running MATLAB examples

The MathWorks has provided useful examples that show how to access the CSV data in meaningful ways. The files are located included in the MATLABBridge-BETA.tar.gz download from above.

Run the examplePlotRadarAndVision.m and examplePlotRadarVisionAndLidar.m applications to see the examples in action.

Tutorials
In a previous section the concept of passing messages to control a vehicle or parrot was discussed. This section further develops some examples of vehicle interaction including path finding and ground plane detection.

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. Make and run 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

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.

Ground Plane Detection Tutorial

This tutorial will introduce you to a common process for implementing perception algorithms, as well as using logfile data as an algorithm development acceleration tool.

1. Ground plane detection overview

The ground plane detection node is an example of a PolySync perception algorithm.

This node depends on a LiDAR point cloud as an input, so it subscribes to the ps_lidar_points_msg to receive a copy of every LiDAR message that’s seen on the PolySync bus .

For each received message, this node will access the point cloud and ignore any non-ground points using simple filtering .

LiDAR ground points are packaged into a separate ps_lidar_points_msg message, which is re-published to the bus for all other nodes to optionally consume.

studio-ground-plane-visualization

viewer-lite-ground-plane-visualization

2. Locate the C++ code

You can find the C++ Ground Plane Detection code as part of our public C++ examples repo here.

PolySync-Core-CPP-Examples/GroundPlaneDetection

2.1 GroundPlaneDetection.cpp

#include < PolySyncNode.hpp >
#include < PolySyncDataModel.hpp >

using namespace std;

/class GroundPlaneDetection : public polysync::Node
{
private:
    ps_msg_type _messageType;

    std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints;

public:
    / void initStateEvent() override
    {
        _messageType = getMessageTypeByName( "ps_lidar_points_msg" );
        registerListener( _messageType );
    }
    void okStateEvent() override
    {
        polysync::datamodel::LidarPointsMessage groundPlaneMessage ( *this );
        groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );
        groundPlaneMessage.setPoints( _groundPlanePoints );
        groundPlaneMessage.publish();
        usleep(50);
    }
    / virtual void messageEvent( std::shared_ptr< polysync::Message > message )
    {
        using namespace polysync::datamodel;
        if( std::shared_ptr <LidarPointsMessage > lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) )
        {
           if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() )
            {
                _groundPlanePoints.clear();
                groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );
                std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints();
                std::vector< polysync::datamodel::LidarPoint > groundPlanePoints;

                std::array< float, 3 > position;

                for( polysync::datamodel::LidarPoint point : lidarPoints )
                {
                    position = point.getPosition();
                    {
                        _groundPlanePoints.push_back( point );
                    }
                }
                //groundPlaneMessage.setPoints( _groundPlanePoints );

              // groundPlaneMessage.publish();

                groundPlanePoints.clear();
                lidarPoints.clear();
            }
        }
    }

    bool pointIsNearTheGround( const std::array< float, 3 > & point )
    {

        return point[0] >= 2.5 and      // x is 2.5+ meters from the vehicle origin
               point[0] < 35 and        // x is less than 35 meters from the vehicle origin
               point[1] > -12 and       // y is greater than -12 meters from the vehicle origin (towards the passenger side)
               point[1] < 12 and        // y is less than 12 meters from the vehicle origin (towards the driver side)
               point[2] > -0.35 and     // z is greater than -0.35 meters from the vehicle origin (towards the ground),

               point[2] < 0.25;         // z is less than 0.25 meters from the vehicle origin
    }

};

/int main()
{
    return 0;
}

2.2 GroundPlaneDetection.cpp explained

As we saw above, the _messageType is used to hold the integer value for the ps_lidar_points_msg.

_groundPlanePoints is used to hold the subset of LiDAR points that represent the ground plane.

private:
    ps_msg_type _messageType;

    std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints;

You will call the following functions─almost always in this order─by a node that’s publishing messages to the bus. After creating an instance of the ps_lidar_points_msg, the payload is set. In this instance the ground plane points vector.

Please note that the message header timestamp is set immediately before the publish message function.

void okStateEvent() override
{
    polysync::datamodel::LidarPointsMessage groundPlaneMessage( *this );

    groundPlaneMessage.setPoints( _groundPlanePoints );

    groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );

    groundPlaneMessage.publish();

    usleep(50);
}

Within the messageEvent, you will promote the base class message to a LidarPointsMessage. It’s important that the node filters out any message that it publishes to the bus.

Once the node receives a valid, new LiDAR points message it can clear out the existing vector of LiDAR ground points.

if( std::shared_ptr <LidarPointsMessage > lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) )
{
    // Filter out this nodes own messages
    if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() )
    {
        _groundPlanePoints.clear();

Each time the node receives a LiDAR point message, it creates another instance of the LiDAR point message to hold the ground plane points. The original message and ground plane points message will have the same message start/end scan timestamps, which allows other node(s) to correlate the original and ground plane messages for further processing.

This code block shows how to create local containers for the incoming message, and iterate over the message payload in the for loop.

LidarPointsMessage groundPlaneMessage ( *this );

groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() );

// Get the entire LiDAR point cloud from the incoming message
std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints();

// Create a container that will hold all ground plane points that are found in the nodes processing
std::vector< polysync::datamodel::LidarPoint > groundPlanePoints;

// Create a container to hold a single point as the node iterates over the full point cloud
std::array< float, 3 > position;

for( polysync::datamodel::LidarPoint point : lidarPoints )
{
    // Get the x/y/z position for this point in the point cloud
    position = point.getPosition();


    if( pointIsNearTheGround( position ) )
    {
        // This point is close the ground, place it in our point vector
        _groundPlanePoints.push_back( point );
    }
}

Since the incoming data has been transformed to a vehicle centered reference coordinate frame by default, we can use the following simple filtering techniques to determine which points in the LiDAR cloud are ground points.

bool pointIsNearTheGround( const std::array< float, 3 > & point )
{
    // The vehicle origin is at the center of the rear axle, on the ground
    // Incoming LiDAR point messages have been corrected for sensor mount position already


    return point[0] >= 2.5 and      // x is 2.5+ meters from the vehicle origin
           point[0] < 35 and        // x is less than 35 meters from the vehicle origin
           point[1] > -12 and       // y is greater than -12 meters from the vehicle origin (towards the passenger side)
           point[1] < 12 and        // y is less than 12 meters from the vehicle origin (towards the driver side)
           point[2] > -0.35 and     // z is greater than -0.35 meters from the vehicle origin (towards the ground),
                                    // this compensates for vehicle pitch as the vehicle drives
           point[2] < 0.25;         // z is less than 0.25 meters from the vehicle origin
}

3. Build and run ground plane node

You can find the C++ Data Generation code as part of our public C++ examples repo here. You will build the node using cmake and make.

$ cd PolySync-Core-CPP-Examples/GroundPlaneDetection`
$ cmake . && make
$ ./polysync-ground-plane-detection-cpp

The node will quietly wait until LiDAR data is received in the messageEvent.

Since this node requires LiDAR data as an input, it won’t do anything unless LiDAR data─ps_lidar_points_msg─is being published to the bus by another node.

The PolySync development lifecycle uses the powerful replay tools to recreate real-world scenarios on the bench.

Using PolySync Studio, which leverages the Record & Replay API, you can command active nodes to replay data from a logfile and publish high-level messages to the bus. This allows for the development of algorithms that subscribe to messages and process the data contained within the messages in real-time.

4. Replaying data

In order to begin replaying data, you will need to ensure the network is setup by setting the PolySync IP address to the loopback interface. Next, you will open the SDF Configurator to make sure the network configuration is valid. If a mismatch is detected, the SDF Configurator will open a host setup wizard .

$ polysync-core-manager -s 127.0.0.1
$ polysync-core-sdf-configurator

With a valid host configuration, you can instruct the PolySync manager to spawn all nodes that are defined on this host and enabled in the SDF.

$ polysync-core-manager -n -w

Now is the time to start PolySync Studio in order to visualize the runtime status of our nodes, and eventually to visualize the LiDAR point data. The System Hierarchy plugin can be opened from the plugin launcher on the right-hand panel to ensure all nodes are in the “OK” state.

$ polysync-core-studio

Next we select the replay tab from the right-hand panel, and select a replay session for playback by double-clicking. The default session is 1000.

The play button becomes solid black when nodes are ready for playback.

5. Visualizing data

The data can be visualized in Studio’s 3D View and the OpenGL based Viewer Lite example node.

Open the 3D View plugin from the plugin launcher.

5.1 Viewer Lite

Viewer Lite is an OpenGL 2D visualizer node that allows us to see primitive data types (LiDAR points, objects, and RADAR targets) being transmitted using the PolySync bus and messages.

To run PolySync Viewer Lite, use the command:

$ polysync-viewer-lite

Conclusion

Congratulations! You have successfully implemented perception algorithms and walked through using logfile data as an algorithm development acceleration tool.

Path Planner Tutorial

This tutorial will show you how to build a basic path planner algorithm in PolySync. We will apply a basic A* planner to a simulated search space, generate the optimal path to the goal state, and then move a simulated robot along that path to the goal. The optimal path is sent one waypoint at a time as a PlatformMotionMessage, which is defined in the C++ Sensor Data Model Module.

1. Search algorithm

A* Search is a classical robotic search technique that combines core facets from two fundamental approaches: the goal-oriented heuristic approach of Greedy with the exhaustive, path-cost approach of Dijkstra’s. The synthesis of the two provides the optimal path with a reduced computational overhead. Check this link for more information on how A* works.

1.1 Robot and searcher

In this tutorial, a robot is plotted on a map and tasked to find the optimal path to a chest of gold while reducing the cost of travel. The robot is given no prior information about its world, other than a heuristic, or estimate, of where the gold is located. The robot can move in eight directions: up, down, right, left, and diagonally. Each time the robot moves, the path cost is increased by one, and the optimal path will be the path that most minimizes the path cost.

PathPlanner

As the robot searches the map using A*, it plots paths towards the goal using the heuristic cost to guide it. As it explores a cell, it adds its estimate of the path cost at that cell to the actual path cost, which is called the global score. The optimal path will have the lowest score along its trajectory. Once the searcher explores a cell, it removes it from an open set (cells to be explored) and adds it to a closed set (nodes that have been explored). It then chooses to explore the next cell with the lowest estimated score and repeats until it has found the goal state.

1.2 Robot and searcher as PolySync nodes

PolySync can be used to bridge the communication between a path planner and a robot. In this example, once the path planner finishes finding the optimal path, it will publish a set of waypoints for the robot to move to. In PolySync, this means that the two nodes (searcher and robot) subscribe to different PlatformMotionMessages containing a set of x and y coordinates.

  • The searcher node publishes a PlatformMotionMessage with a set of x and y coordinates.
  • The robot node subscribed to that message type moves to the desired location.
  • Upon arrival, the robot node publishes its own PlatformMotionMessage, indicating its current position.
  • The searcher node subscribed to the same message type receives the robot’s current location and then issues the next waypoint in the form of a new PlatformMotionMessage.
  • This repeats until the robot reports back that it has found the gold.

2. Setup the example

You can find the C++ Path Planner code as part of our public C++ examples repo here.

PolySync-Core-CPP-Examples/PathPlanner/
├── include
│   ├── GridMap.hpp
│   ├── Planner.hpp
│   ├── RobotNode.hpp
│   └── SearchNode.hpp
├── resources
│   ├── gold.jpg
│   ├── robot.jpg
│   └── maze2.pgm
├── CMakeLists.txt
├── RobotNode.cpp
├── SearchNode.cpp
└── src
    ├── GridMap.cpp
    └── Planner.cpp

2.1 OpenCV installation

This example requires OpenCV to render the map. If you have already installed OpenCV, you can move to the next section. If not, please review their documentation here.

2.2 Running the example

Build the example by calling cmake on CMakeLists.txt in the PathPlanner directory.

$ cd PolySync-Core-CPP-Examples/PathPlanner
$ mkdir build
$ cd build
$ cmake ..
$ make

Since the program is split between two nodes, you’ll need to run them in separate terminal windows. Since the robot node first acts as a listener, you should run that node first.

$ ./polysync-path-planner-robot-cpp

In another terminal window, run the planner node.

$ ./polysync-path-planner-algorithm-cpp

2.3 What’s happening?

When the robot node is constructed, it knows where the robot is located, but not where the goal is. Conversely, when the search node is constructed, it generates a goal state, but needs a start location.

By using a new PlatformMotionMessage to communicate back and forth, the search node sends a goal location to the robot node, who plots the goal state and returns to the starting robot location. Once the search node receives the robot’s location, it can then begin searching the map for the optimal path.

PathPlanner

After several seconds, the search node will produce the optimal path and will ask you to push the return key to execute. You will then be able to watch the robot go for the gold.

PathPlanner

PathPlanner

3. Code breakdown

SearchNode.cpp and RobotNode.cpp are where the PolySync nodes are defined, and where you can access the main entry points for this application. The SearchNode will generate an instance of a Planner class to search for the optimal path. The RobotNode, on the other hand, will create a GridMap object to display its current location.

3.1 Finding RobotNode code

You can find the C++ Robot Node code as part of the Path Planner project in our public C++ examples repo here.

3.2 The RobotNode.cpp code

#include <cmath>
#include <iostream>

#include <PolySyncDataModel.hpp>

#include "RobotNode.hpp"

using namespace cv;
using namespace std;
using namespace polysync::datamodel;

constexpr int INVALID_LOC = -1;


RobotNode::RobotNode( )
    :
    _world( ),
    _golLocX( INVALID_LOC ),
    _golLocY( INVALID_LOC ),
    _newRobLocX( INVALID_LOC ),
    _newRobLocY( INVALID_LOC ),
    _numWaypoints( INVALID_LOC ),
    _waypointCounter( 0 )
{
    setNodeName( "robotNode" );
}


RobotNode::~RobotNode( )
{
}


void RobotNode::initStateEvent( )
{

    // register node as a subscriber to platform motion messages from ANY node.
    registerListener( getMessageTypeByName( "ps_platform_motion_msg" ) );

    setSubscriberReliabilityQOS(
            getMessageTypeByName( "ps_platform_motion_msg" ),
            RELIABILITY_QOS_RELIABLE );
}


void RobotNode::okStateEvent( )
{

    // wait for a goal location message from searchNode to be received
    if ( _golLocX == INVALID_LOC || _golLocY == INVALID_LOC )
    {

        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro(10000);
        return;

    }

    // when goal received, generate a map with that goal state.
    else if ( _newRobLocX == INVALID_LOC || _newRobLocY == INVALID_LOC )
    {

        cout << endl << "Goal location received by robot.";
        cout << endl << "Generating map - - - - - - > " << std::flush;

        _world = std::unique_ptr< GridMap >{ new GridMap };

        // pass in goal location
        _world->generateMap( _golLocX, _golLocY );

        _newRobLocX = _world->robLoc[0][0];
        _newRobLocY = _world->robLoc[0][1];

        cout << "Map generated. " << endl;
        cout << "Sending robot start location to planner algorithm." << endl;
        cout << endl << "RobotNode waiting for waypoints." << endl;

    }

    // with map generated, begin sending location messages to searchNode
    else if ( _golLocX != INVALID_LOC && _golLocY != INVALID_LOC )
    {

        sendLocationToPlanner( );

        // have I recieved the final waypoint?  if so, ask the user to shut down
        if ( _waypointCounter == _numWaypoints - 2)
        {
            cout << endl << "Robot is at goal state after ";
            cout << _waypointCounter << " waypoints. " <<  endl << endl;
            cout << "Press return key or Ctrl-C to shut down RobotNode.";
            cout << endl;

            cin.get();
            cout << endl << "Shutting down RobotNode." << endl << endl;

            disconnectPolySync( );

            return;
        }

        // The ok state is called periodically by the system so sleep to reduce
        // the number of messages sent. do nothing, sleep for 1 millisecond.
        polysync::sleepMicro(1000);

    }

    else
    {

        // do nothing, sleep for 100 milliseconds
        polysync::sleepMicro(100000);
        return;

    }
}


void RobotNode::messageEvent( std::shared_ptr< polysync::Message > newMsg )
{

    // check whether new message is not your own. This check is only important
    // since robotNode and searchNode both publish and subscribe to messages.
    if ( newMsg->getSourceGuid( ) == getGuid( ) )
    {
        return;
    }

    // now check whether new message is a PlatformMotionMessage.
    if ( auto msg = getSubclass< PlatformMotionMessage >( newMsg ) )
    {

        // the first received message from searchNode will be the goal location.
        if ( _golLocX == INVALID_LOC && _golLocY == INVALID_LOC )
        {
            _golLocX = msg->getOrientation()[0];
            _golLocY = msg->getOrientation()[1];
        }

        // discard any received messages still containing the goal location
        else if ( msg->getOrientation()[0] == _golLocX &&
                  msg->getOrientation()[1] == _golLocY )
        {
            return;
        }

        // if a new waypoint, send it to the robot and move to the new location.
        else if ( msg->getPosition()[0] > _waypointCounter )
        {
            _waypointCounter = msg->getPosition()[0];
            _numWaypoints = msg->getPosition()[2];

            _newRobLocX = msg->getOrientation()[0];
            _newRobLocY = msg->getOrientation()[1];

            cout << "Received waypoint " << _waypointCounter;
            cout << " from Planner.  X = " << _newRobLocX << ", Y = ";
            cout << _newRobLocY << endl << std::flush;
        }
    }
}


void RobotNode::sendLocationToPlanner( )
{

    _world->moveRobot( _newRobLocX, _newRobLocY );

    double actualRobLocX = double( _world->robLoc[0][0] );
    double actualRobLocY = double( _world->robLoc[0][1] );

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setPosition( { _waypointCounter, 0, 0 } );
    msg.setOrientation( { actualRobLocX, actualRobLocY, 0, 0 } );

    // Publish to the PolySync bus
    msg.publish();

}


int main( )
{

    RobotNode robotNode;
    robotNode.connectPolySync( );

    return 0;
}

3.3 The RobotNode.cpp code explained

Now let’s break down the RobotNode.cpp code.

The line below defines the PolySync node and each of the protected event methods in the node state machine. Each of the states do minimal or no operation unless they are overridden.

#include < PolySyncNode.hpp >

The following data model defines all types and messages in the PolySync runtime. This must be included in all C++ node applications.

#include < PolySyncDataModel.hpp >

The following are specific to this application.

#include "RobotNode.hpp"

using namespace cv;
using namespace std;
using namespace polysync::datamodel;

constexpr int INVALID_LOC = -1;

The constructor initializes all private members.

RobotNode::RobotNode( )
    :
    _world( ),
    _golLocX( INVALID_LOC ),
    _golLocY( INVALID_LOC ),
    _newRobLocX( INVALID_LOC ),
    _newRobLocY( INVALID_LOC ),
    _numWaypoints( INVALID_LOC ),
    _waypointCounter( 0 )
{
    setNodeName( "robotNode" );
}

RobotNode::~RobotNode( )
{
}

All of our PolySync C++ applications must then subclass the PolySync node object in order to connect to the PolySync bus and publish/subscribe messages. In RobotNode.hpp, you can see how RobotNode inherits a PolySync node object.

class RobotNode : public polysync::Node

Back in RobotNode.cpp, after the constructor is called and the node is generated, the node transitions to the INIT state. The initStateEvent is typically where a node creates messages and initializes any other resources that require a PolySync node reference. In this case, we register the RobotNode to listen for ps_platform_motion_msg.

void RobotNode::initStateEvent( )
{

    // register node as a subscriber to platform motion messages from ANY node.
    registerListener( getMessageTypeByName( "ps_platform_motion_msg" ) );

    setSubscriberReliabilityQOS(
            getMessageTypeByName( "ps_platform_motion_msg" ),
            RELIABILITY_QOS_RELIABLE );
}

The node then transitions from the INIT state to the OK state. The following is called continuously while in the node’s OK state, and is where almost all of the action happens for the node.

void RobotNode::okStateEvent( )

If the robot node has not received a goal state, do nothing and loop through again.

    if ( _golLocX == INVALID_LOC || _golLocY == INVALID_LOC )
    {

        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro(10000);
        return;

    }

If the goal has been received, the robot node can now generate the map. This happens here.

    else if ( _newRobLocX == INVALID_LOC || _newRobLocY == INVALID_LOC )
    {

        cout << endl << "Goal location received by robot.";
        cout << endl << "Generating map - - - - - - > " << std::flush;

        _world = std::unique_ptr< GridMap >{ new GridMap };

        // pass in goal location
        _world->generateMap( _golLocX, _golLocY );

        _newRobLocX = _world->robLoc[0][0];
        _newRobLocY = _world->robLoc[0][1];

        cout << "Map generated. " << endl;
        cout << "Sending robot start location to planner algorithm." << endl;
        cout << endl << "RobotNode waiting for waypoints." << endl;

    }

Once the map is generated, the robot node can wait for waypoints to arrive. This chunk of code is what will execute whenever SearchNode is sending waypoints and the robot is moving. If the robot arrives at the goal state, the disconnectPolySync( ) is called and the node shuts down.

    else if ( _golLocX != INVALID_LOC && _golLocY != INVALID_LOC )
    {

        sendLocationToPlanner( );

        // have I recieved the final waypoint?  if so, ask the user to shut down
        if ( _waypointCounter == _numWaypoints - 2)
        {
            cout << endl << "Robot is at goal state after ";
            cout << _waypointCounter << " waypoints. " <<  endl << endl;
            cout << "Press return key or Ctrl-C to shut down RobotNode.";
            cout << endl;

            cin.get();
            cout << endl << "Shutting down RobotNode." << endl << endl;

            disconnectPolySync( );

            return;
        }

        // The ok state is called periodically by the system so sleep to reduce
        // the number of messages sent. do nothing, sleep for 1 millisecond.
        polysync::sleepMicro(1000);

    }
}

The messageEvent is called whenever a new message is received.

void RobotNode::messageEvent( std::shared_ptr< polysync::Message > newMsg )

When a new message is received, the first thing to do is to check whether it is a message published by the same node. In this case, RobotNode should discard anything it self-published.

    if ( newMsg->getSourceGuid( ) == getGuid( ) )
    {
        return;
    }

The next line checks whether the received message is a PlatformMotionMessage. This is technically unnecessary in our case, since this problem is only structured to send and receive PlatformMotionMessage, but it is good practice as you scale up your application.

   if ( auto msg = getSubclass< PlatformMotionMessage >( newMsg ) )

After the robot node verifies it has a new PlatformMotionMessage from a different node, it can now classify whether it is the goal state (the first received message) or a new waypoint. Local variables are populated accordingly.

        // the first received message from searchNode will be the goal location.
        if ( _golLocX == INVALID_LOC && _golLocY == INVALID_LOC )
        {
            _golLocX = msg->getOrientation()[0];
            _golLocY = msg->getOrientation()[1];
        }

        // discard any received messages still containing the goal location
        else if ( msg->getOrientation()[0] == _golLocX &&
                  msg->getOrientation()[1] == _golLocY )
        {
            return;
        }

        // if a new waypoint, send it to the robot and move to the new location.
        else if ( msg->getPosition()[0] > _waypointCounter )
        {
            _waypointCounter = msg->getPosition()[0];
            _numWaypoints = msg->getPosition()[2];

            _newRobLocX = msg->getOrientation()[0];
            _newRobLocY = msg->getOrientation()[1];

            cout << "Received waypoint " << _waypointCounter;
            cout << " from Planner.  X = " << _newRobLocX << ", Y = ";
            cout << _newRobLocY << endl << std::flush;
        }

This shows how the robot updates the SearchNode with its current location. Once the new location is received, SearchNode publishes the next waypoint.

void RobotNode::sendLocationToPlanner( )
{

    _world->moveRobot( _newRobLocX, _newRobLocY );

    double actualRobLocX = double( _world->robLoc[0][0] );
    double actualRobLocY = double( _world->robLoc[0][1] );

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setPosition( { _waypointCounter, 0, 0 } );
    msg.setOrientation( { actualRobLocX, actualRobLocY, 0, 0 } );

    // Publish to the PolySync bus
    msg.publish();

}

This creates an instance of your RobotNode object. The second command is a blocking operation, and places the node in the node state machine. If there is a valid license, the node leaves the AUTH state and enters the INIT state, calling the initStateEvent.

int main( )
{
    RobotNode robotNode;
    robotNode.connectPolySync( );

    return 0;
}

3.4 Finding SearchNode code

You can find the C++ SearchNode.cpp code as part of the Path Planner project in our public C++ examples repo here.

3.5 The SearchNode.cpp code

#include <cmath>
#include <iostream>

#include <PolySyncDataModel.hpp>

#include "SearchNode.hpp"
#include "GridMap.hpp"

using namespace cv;
using namespace std;
using namespace polysync::datamodel;

constexpr int INVALID_LOC = -1;


SearchNode::SearchNode( )
    :
    _searcher( ),
    _golLocX( INVALID_LOC ),
    _golLocY( INVALID_LOC ),
    _robLocX( INVALID_LOC ),
    _robLocY( INVALID_LOC ),
    _newRobLocX( INVALID_LOC ),
    _newRobLocY( INVALID_LOC ),
    _numWaypoints( INVALID_LOC ),
    _waypointCounter( INVALID_LOC )
{
    setNodeName( "searchNode" );

    setNodeType( PSYNC_NODE_TYPE_SOFTWARE_ALGORITHM );
}

SearchNode::~SearchNode( )
{
}


void SearchNode::initStateEvent( )
{

    // register node as a subscriber to platform motion messages from ANY node.
    registerListener( getMessageTypeByName( "ps_platform_motion_msg" ) );

    setSubscriberReliabilityQOS(
            getMessageTypeByName( "ps_platform_motion_msg" ),
            RELIABILITY_QOS_RELIABLE );
}


void SearchNode::okStateEvent( )
{

    // generate goal state at a pseudo-random location.
    if ( _golLocX == INVALID_LOC && _golLocY == INVALID_LOC )
    {

        _searcher = std::unique_ptr< Planner >{ new Planner };

        _golLocX = _searcher->getGoalX( );
        _golLocY = _searcher->getGoalY( );

        cout << endl << "Goal Location generated by Planner Algorithm. ";
        cout << endl << "Sending goal location to robot." << endl << endl;
        cout << "Waiting for Robot Location." << endl << endl << std::flush;

    }

    // send goal location to robot repeatedly until it is received.
    else if ( _robLocX == INVALID_LOC || _robLocY == INVALID_LOC )
    {

        sendGoalToRobot( );

        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro(10000);

    }

    // once robot reports its starting location, search the space for the
    // optimal path from start to goal state.
    else if ( _newRobLocX == INVALID_LOC && _newRobLocY == INVALID_LOC )
    {

        cout << "Robot start location received by planner algorithm." << endl;
        cout << "Begin searching for optimal path from start location." << endl;

        int robIndex = _searcher->world.getIndexFromState( _robLocX, _robLocY) ;

        // use A* search to find optimal path.
        _numWaypoints = _searcher->searchAStar( robIndex );

        _newRobLocX = int(_robLocX);
        _newRobLocY = int(_robLocY);

    }

    // wait until done searching, then send out next waypoint.
    else if ( _newRobLocX != INVALID_LOC || _newRobLocY != INVALID_LOC )
    {

        // have I sent the final waypoint? if so, shut down
        if ( _waypointCounter == _numWaypoints - 2 )
        {
            cout << endl << "Robot arrived at goal state after ";
            cout << _waypointCounter << " waypoints. " <<  endl;
            cout << "Shutting down SearchNode." << endl << endl;

            disconnectPolySync( );

            return;
        }

        cout << "Sending waypoint " << _waypointCounter + 1 << " to robot.";
        cout << endl;

        int newIndex = _searcher->getNextWaypoint( _waypointCounter + 1 );

        sendNextWaypoint( newIndex, int(_waypointCounter + 1) );

        // The ok state is called periodically by the system so sleep to reduce
        // the number of messages sent. do nothing, sleep for 1 millisecond.
        polysync::sleepMicro(1000);

    }

    else
    {

        // do nothing, sleep for 100 milliseconds
        polysync::sleepMicro(100000);
        return;

    }
}


void SearchNode::messageEvent( std::shared_ptr< polysync::Message > newMsg )
{

    // check whether new message is not your own. This check is only important
    // since robotNode and searchNode both publish and subscribe to messages.
    if ( newMsg->getSourceGuid( ) == getGuid( ) )
    {
        return;
    }

    // now check whether new message is a PlatformMotionMessage.
    if ( auto msg = getSubclass< PlatformMotionMessage >( newMsg ) )
    {

        // all received platform motion messages will be current robot location.
        // robot will also report back last waypoint received so planner can
        // send the next waypoint.
        if ( msg->getOrientation()[0] != _robLocX ||
                msg->getOrientation()[1] != _robLocY )
        {
            _robLocX = msg->getOrientation()[0];
            _robLocY = msg->getOrientation()[1];

            if ( _waypointCounter != INVALID_LOC )
            {
                cout << "New Robot Location Message received at waypoint: ";
                cout << msg->getPosition()[0] << endl << std::flush;
            }
            _waypointCounter = msg->getPosition()[0];
        }
    }
}


void SearchNode::sendGoalToRobot( )
{

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setOrientation( { double(_golLocX), double(_golLocY), 0, 0 } );

    // Publish to the PolySync bus
    msg.publish( );

}


void SearchNode::sendNextWaypoint( int newIndex, int waypointID )
{

    _searcher->world.getStateFromIndex( newIndex );
    _newRobLocX = _searcher->world.checkedMoveIndX;
    _newRobLocY = _searcher->world.checkedMoveIndY;

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setPosition( { double(waypointID), 0, double(_numWaypoints) } );
    msg.setOrientation( { _newRobLocX, _newRobLocY, 0, 0 } );

    // Publish to the PolySync bus
    msg.publish();

}


int main()
{

    SearchNode searchNode;
    searchNode.connectPolySync( );

    return 0;
}

3.6 The SearchNode.cpp code explained

Now let’s take a look at the SearchNode.cpp code.

The line below defines the PolySync node and each of the protected event methods in the node state machine. Each of the states do minimal or no operation unless they are overridden.

#include < PolySyncNode.hpp >

The following data model defines all types and messages in the PolySync runtime. This must be included in all C++ node applications.

#include < PolySyncDataModel.hpp >

The following are needed for this application.

#include "SearchNode.hpp"
#include "GridMap.hpp"

using namespace cv;
using namespace std;
using namespace polysync::datamodel;

constexpr int INVALID_LOC = -1;

The constructor initializes all private members.

SearchNode::SearchNode( )
    :
    _searcher( ),
    _golLocX( INVALID_LOC ),
    _golLocY( INVALID_LOC ),
    _robLocX( INVALID_LOC ),
    _robLocY( INVALID_LOC ),
    _newRobLocX( INVALID_LOC ),
    _newRobLocY( INVALID_LOC ),
    _numWaypoints( INVALID_LOC ),
    _waypointCounter( INVALID_LOC )
{
    setNodeName( "searchNode" );

    setNodeType( PSYNC_NODE_TYPE_SOFTWARE_ALGORITHM );
}

SearchNode::~SearchNode( )
{
}

All of our PolySync C++ applications must then subclass the PolySync node object in order to connect to the PolySync bus and publish/subscribe messages. In SearchNode.hpp, you can see how SearchNode inherits a PolySync node object.

class SearchNode : public polysync::Node

Back in SearchNode.cpp, after the constructor is called and the node is generated, the node transitions to the INIT state. The initStateEvent is typically where a node creates messages and initializes any other resources that require a PolySync node reference. In this case, we register the SearchNode to listen for ps_platform_motion_msg.

void SearchNode::initStateEvent( )
{

    // register node as a subscriber to platform motion messages from ANY node.
    registerListener( getMessageTypeByName( "ps_platform_motion_msg" ) );

    setSubscriberReliabilityQOS(
            getMessageTypeByName( "ps_platform_motion_msg" ),
            RELIABILITY_QOS_RELIABLE );
}

The node then transitions from the INIT state to the OK state. The following is called continuously while in the node’s OK state, and is where almost all of the action happens for the node.

void SearchNode::okStateEvent( )

Generate a goal state, then do nothing and loop through again.

    if ( _golLocX == INVALID_LOC && _golLocY == INVALID_LOC )
    {

        _searcher = std::unique_ptr< Planner >{ new Planner };

        _golLocX = _searcher->getGoalX( );
        _golLocY = _searcher->getGoalY( );

        cout << endl << "Goal Location generated by Planner Algorithm. ";
        cout << endl << "Sending goal location to robot." << endl << endl;
        cout << "Waiting for Robot Location." << endl << endl << std::flush;

    }

Once generated, publish the goal state to the robot node until it is received.

    else if ( _robLocX == INVALID_LOC || _robLocY == INVALID_LOC )
    {

        sendGoalToRobot( );

        // do nothing, sleep for 10 milliseconds
        polysync::sleepMicro(10000);

    }

If the goal has been received, and the map generated, the robot sends its start location to the search node. Once the search node receives the start location, it can search for the optimal path.

    else if ( _newRobLocX == INVALID_LOC && _newRobLocY == INVALID_LOC )
    {

        cout << "Robot start location received by planner algorithm." << endl;
        cout << "Begin searching for optimal path from start location." << endl;

        int robIndex = _searcher->world.getIndexFromState( _robLocX, _robLocY) ;

        // use A* search to find optimal path.
        _numWaypoints = _searcher->searchAStar( robIndex );

        _newRobLocX = int(_robLocX);
        _newRobLocY = int(_robLocY);

    }

Once the search node has completed searching the grid for the optimal path, each successive loop through the okStateEvent( ) will publish a new waypoint for the robot node to move to. If the next waypoint is at the goal state, the disconnectPolySync( ) is called and the node shuts down.

    else if ( _newRobLocX != INVALID_LOC || _newRobLocY != INVALID_LOC )
    {

        // have I sent the final waypoint?  if so, shut down
        if ( _waypointCounter == _numWaypoints - 2 )
        {
            cout << endl << "Robot arrived at goal state after ";
            cout << _waypointCounter << " waypoints. " <<  endl;
            cout << "Shutting down SearchNode." << endl << endl;

            disconnectPolySync( );

            return;
        }

        cout << "Sending waypoint " << _waypointCounter + 1 << " to robot.";
        cout << endl;

        int newIndex = _searcher->getNextWaypoint( _waypointCounter + 1 );

        sendNextWaypoint( newIndex, int(_waypointCounter + 1) );

        // The ok state is called periodically by the system so sleep to reduce
        // the number of messages sent. do nothing, sleep for 1 millisecond.
        polysync::sleepMicro(1000);

    }

The messageEvent is called whenever a new message is received.

void SearchNode::messageEvent( std::shared_ptr< polysync::Message > newMsg )

When a new message is received, the first thing to do is to check whether it is a message published by the same node. In this case, SearchNode should discard anything it has self-published.

    if ( newMsg->getSourceGuid( ) == getGuid( ) )
    {
        return;
    }

The next line checks whether the received message is a PlatformMotionMessage. This is technically unnecessary in our case, since this problem is only structured to send and receive PlatformMotionMessage, though it is good practice as you scale up your application.

   if ( auto msg = getSubclass< PlatformMotionMessage >( newMsg ) )

After the search node verifies it has a new PlatformMotionMessage from a different node, it can now check whether the robot has moved to a new location. Local variables are populated accordingly.

        if ( msg->getOrientation()[0] != _robLocX ||
                msg->getOrientation()[1] != _robLocY )
        {
            _robLocX = msg->getOrientation()[0];
            _robLocY = msg->getOrientation()[1];

            if ( _waypointCounter != INVALID_LOC )
            {
                cout << "New Robot Location Message received at waypoint: ";
                cout << msg->getPosition()[0] << endl << std::flush;
            }
            _waypointCounter = msg->getPosition()[0];
        }

This shows how the searcher updates the RobotNode with the goal state. This method is called repeatedly in the okStateEvent until the RobotNode has received it and reported back its starting position.

void SearchNode::sendGoalToRobot( )
{

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setOrientation( { double(_golLocX), double(_golLocY), 0, 0 } );

    // Publish to the PolySync bus
    msg.publish( );

}

This shows how the search node updates the RobotNode with the next waypoint. Once the new location is received, RobotNode moves the robot to the new location and then publishes its current position.

void SearchNode::sendNextWaypoint( int newIndex, int waypointID )
{

    _searcher->world.getStateFromIndex( newIndex );
    _newRobLocX = _searcher->world.checkedMoveIndX;
    _newRobLocY = _searcher->world.checkedMoveIndY;

    // Create a message
    PlatformMotionMessage msg( *this );

    // Set publish time
    msg.setHeaderTimestamp( polysync::getTimestamp() );

    // Populate buffer
    msg.setPosition( { double(waypointID), 0, double(_numWaypoints) } );
    msg.setOrientation( { _newRobLocX, _newRobLocY, 0, 0 } );

    // Publish to the PolySync bus
    msg.publish();

}

This creates an instance of your SearchNode object. The second command is a blocking operation, and places the node in the node state machine. If there is a valid license, the node leaves the AUTH state and enters the INIT state, calling the initStateEvent.

int main()
{

    SearchNode searchNode;
    searchNode.connectPolySync( );

    return 0;
}

3.7 Planner code

This example uses A* to search for an optimal path. Of course, there are many different ways to search. See below how we implemented A*, and for extra credit, try writing your own algorithm.

3.8 Planner::searchAStar( )

int Planner::searchAStar( int _curLoc )
{

    // start the clock to time performance.
    float beginTime = clock();

    // reset expandedNodes counter
    _expandedNodes = 1;

    cout << endl << "Searching with A* . " << std::flush;

    while ( _endGame != true )
    {

        // expand the node in the openSet with lowest current score.
        _curLoc = getSearchNode( );

        // get x and y coordinates
        world.getStateFromIndex( _curLoc );

        // find all legal moves at the current node
        getNeighbors( _curLoc );

        // grade all legal moves to fully expand a node
        for (uint j = 0; j < _moves[ _curLoc ].size(); j++)
        {

            // set move to be graded
            _newLoc = _moves[ _curLoc ][ j ];

            // A* guarantees optimality along all expanded nodes, so there is no
            // need to re-check. If move has been expanded, move along
            if ( std::find( _closedSet.begin(), _closedSet.end(), _newLoc )
                            != _closedSet.end() )
            {
                continue;
            }

            // if move is at goal state, end game and send back path
            if ( world.checkGoal( _newLoc ))
            {
                // path @ newLoc will be the optimal path
                path[ _newLoc ] = path[ _curLoc ];
                path[ _newLoc ].push_back( _newLoc );
                _curLoc = _newLoc;
                _endGame = true;
                goto doneSearching;

            }
            else
            {

                // do nothing unless the new path to the index is less than the
                // current shortest path. This ensures that the global score is
                // only modified if the searcher has found a shorter path.
                if ( path[ _curLoc ].size() + 1 < _pathScore[ _newLoc ] )
                {
                    path[ _newLoc ] = path[ _curLoc ];

                    path[ _newLoc ].push_back( _newLoc );

                    _globalScore[ _newLoc ] =
                                    _heuristic[ _newLoc ]
                                    + path[ _newLoc ].size();

                    _pathScore[ _newLoc ] = path[ _newLoc ].size();
                }
            }
        }

        // once fully expanded, set the globalScore at the expanded node to a
        // large number. This is done to discourage searching the same node.
        _globalScore[ _curLoc ] = 1e9;

        // erase the current node from the openSet (nodes to be expanded).
        _openSet.erase(
                std::remove( _openSet.begin(), _openSet.end(), _curLoc ),
                _openSet.end() );

        // add the current node to the closedSet (nodes that have been expanded)
        _closedSet.push_back( _curLoc );

        // increase expanded node counter
        ++_expandedNodes;

        // print a "." for every 1000 nodes expanded as a progress meter
        if ( _expandedNodes % 1000 == 0 )
        {
            cout << ". " << std::flush;
        }
    }

    // what to do at endGame
    doneSearching:

    float endTime = float( clock() - beginTime ) / float( CLOCKS_PER_SEC );

    cout << endl << "Path Optimized, " << _expandedNodes << " nodes expanded.";
    cout << endl << "Total Optimization time: " << endTime << " seconds"<< endl;
    cout << endl << "Press the return key to execute optimal path." << endl;

    cin.get();

    cout << "Optimal path is " << path[ _curLoc ].size() << " steps long.";
    cout << endl;

    return path[ _curLoc ].size();

}

Here’s a visualizer to demonstrate different path planning algorithms in action. If you’d like to learn even more about sample-based planning, check out the Open Motion Planning Library (OMPL). If you have more resources, feel free to post them to the comments section below.

Conclusion

You have just finished the Path Planner Tutorial. Here, you used an A* Implementation on a toy problem to show how a planning algorithm can generate an optimal trajectory and send it to a robot platform. You also used PolySync to model the planner and robot as two separate nodes communicating messages back and forth. These nodes were given no information about the other besides the information received in messages off the PolySync bus. You can use a similar communication structure between nodes on an autonomous vehicle.