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 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 and adds it to a closed set. It chooses to explore the next cell with the lowest estimated score and repeats until it has found the goal state.

1.1 Robot and searcher as PolySync nodes

PolySync Core can be used to bridge the communication between a path planner node and a robot node.

In this example a path planner calculates and publishes the optimal path as a set of waypoints for the robot to travel to.

In PolySync, this means that the two nodes subscribe to 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 receives the robot’s current location via the PlatformMotionMessage and issues the next waypoint in another instance of the PlatformMotionMessage
  • This repeats until the robot reports that it has found the treasure

2. Setup the example

Find the C++ Path Planner code in the public C++ examples repo.

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.

2.2 Running the example

Build the example with cmake in the PathPlanner directory.

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

The program is split between two nodes that are run as seperate processes. Since the robot node acts as a listener, start it 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.

When the search node is constructed. it generates a goal state but needs a start location.

By using a new PlatformMotionMessage to communicate, the search node sends a goal location to the robot node that plots the goal state and returns to the starting robot location.

Once the search node receives the robot’s location it can 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 define the PolySync nodes, and contain the main entry points for these application.

The SearchNode generates an instance of a Planner class to search for the optimal path.

The RobotNode creates 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 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.

To learn even more about sample-based planning, check out the Open Motion Planning Library (OMPL).

Conclusion

You have just finished the path planner tutorial using 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.