Path Planner Tutorial

### 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](http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html) 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](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/11/2-map.png) 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 The example programs `polysync-path-planner-robot-cpp` and `polysync-path-planner-algorithm-cpp` are included in the PolySync installation. You can find the C++ code by default in `/usr/local/polysync/examples/cpp/PathPlanner`. ```bash /usr/local/polysync/examples/cpp/PathPlanner/ ├── include │ ├── GridMap.hpp │ ├── Planner.hpp │ ├── RobotNode.hpp │ └── SearchNode.hpp ├── resources │ ├── gold.jpg │ ├── robot.jpg │ ├── maze1.png │ ├── maze2.pgm │ └── question.jpg ├── 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](http://docs.opencv.org/2.4/doc/tutorials/introduction/linux_install/linux_install.html). ### 2.2 Running the example Build the example by calling `cmake` on `CMakeLists.txt` in the PathPlanner directory. ```bash $ cd /usr/local/polysync/examples/cpp/PathPlanner $ mkdir build $ cd build $ cmake .. ``` 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. ```bash $ ./polysync-path-planner-robot-cpp ``` In another terminal window, run the planner node. ```bash $ ./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](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/11/5-Terminal.png) 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](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/11/4-map.png) ![PathPlanner](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/11/3-map.png) ### 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++ `RobotNode.cpp` code─by default─in `/usr/local/polysync/examples/cpp/PathPlanner`. ### 3.2 The RobotNode.cpp code ```bash #include #include #include #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](http://polysync.github.io/Docs-2.0/#cpp-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. ```bash #include < PolySyncNode.hpp > ``` The following [data model](http://polysync.github.io/Docs-2.0/#cpp-data-model) defines all types and messages in the PolySync runtime. This **must** be included in all C++ node applications. ```bash #include < PolySyncDataModel.hpp > ``` The following are specific to this application. ```bash #include "RobotNode.hpp" using namespace cv; using namespace std; using namespace polysync::datamodel; constexpr int INVALID_LOC = -1; ``` The constructor initializes all private members. ```bash 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. ```bash 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`. ```bash 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. ```bash void RobotNode::okStateEvent( ) ``` If the robot node has not received a goal state, do nothing and loop through again. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash // 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. ```bash 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`. ```bash int main( ) { RobotNode robotNode; robotNode.connectPolySync( ); return 0; } ``` ### 3.4 Finding SearchNode code You can find the C++ `SearchNode.cpp` code─by default─in `/usr/local/polysync/examples/cpp/PathPlanner`. ### 3.5 The SearchNode.cpp code ```bash #include #include #include #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](http://polysync.github.io/Docs-2.0/#cpp-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. ```bash #include < PolySyncNode.hpp > ``` The following [data model](http://polysync.github.io/Docs-2.0/#cpp-data-model) defines all types and messages in the PolySync runtime. This **must** be included in all C++ node applications. ```bash #include < PolySyncDataModel.hpp > ``` The following are needed for this application. ```bash #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. ```bash 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. ```bash 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`. ```bash 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. ```bash void SearchNode::okStateEvent( ) ``` Generate a goal state, then do nothing and loop through again. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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. ```bash 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`. ```bash 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( )` ```bash 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](https://qiao.github.io/PathFinding.js/visual/) 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)](http://ompl.kavrakilab.org/core/). 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.