# 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. 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 Core 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. 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.  ### 3. Code breakdown

`SearchNode.cpp` and `RobotNode.cpp` define the PolySync Core 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;
_newRobLocY = _world->robLoc;

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();
_golLocY = msg->getOrientation();
}

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

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

_newRobLocX = msg->getOrientation();
_newRobLocY = msg->getOrientation();

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 );
double actualRobLocY = double( _world->robLoc );

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

// Set publish time

// 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 Core 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 Core 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 Core C++ applications must subclass the PolySync Core node object in order to connect to the PolySync Core bus and publish/subscribe messages.

In `RobotNode.hpp`, you can see how `RobotNode` inherits a PolySync Core 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 Core 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;
_newRobLocY = _world->robLoc;

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();
_golLocY = msg->getOrientation();
}

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

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

_newRobLocX = msg->getOrientation();
_newRobLocY = msg->getOrientation();

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 );
double actualRobLocY = double( _world->robLoc );

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

// Set publish time

// 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() != _robLocX ||
msg->getOrientation() != _robLocY )
{
_robLocX = msg->getOrientation();
_robLocY = msg->getOrientation();

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

void SearchNode::sendGoalToRobot( )
{

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

// Set publish time

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

// 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 Core 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 Core 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 Core C++ applications must then subclass the PolySync Core node object in order to connect to the PolySync Core bus and publish/subscribe messages. In `SearchNode.hpp`, you can see how `SearchNode` inherits a PolySync Core 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 Core 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() != _robLocX ||
msg->getOrientation() != _robLocY )
{
_robLocX = msg->getOrientation();
_robLocY = msg->getOrientation();

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

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

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

// 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 Core to model the planner and robot as two separate nodes communicating messages back and forth.