Ground Plane Detection Tutorial

### 1. Ground plane detection overview The ground plane detection node is an example of a PolySync perception algorithm . This node depends on a LiDAR point cloud as an input, so it subscribes to the `ps_lidar_points_msg` to receive a copy of every LiDAR message that's seen on the PolySync bus . For each received message, this node will access the point cloud and ignore any non-ground points using simple filtering . LiDAR ground points are packaged into a separate `ps_lidar_points_msg` message, which is re-published to the bus for all other nodes to optionally consume. ![studio-ground-plane-visualization](https://help.polysync.io/releases/2.0.8//wp-content/uploads/2016/09/studio-ground-plane-visualization.png) ![viewer-lite-ground-plane-visualization](https://help.polysync.io/releases/2.0.8//wp-content/uploads/2016/09/viewer-lite-ground-plane-visualization.png)

All nodes must follow the same timestamping guidelines to maintain a valid, unified time domain: the PolySync time domain. Data is timestamped at the earliest possible moment once it has been received by the Dynamic Driver interface, or produced by a software algorithm. We can assume that all PolySync timestamps will be UTC timestamps unless specifically documented otherwise. The message header timestamp represents when a message is published to the bus by an originating node. The message timestamp represents when the data is received by the node, and the message native timestamp is also set when the connected hardware provides information. Messages that are published to the bus should always populate the available and valid timestamp fields, as shown in this tutorial. This node re-publishes data that has been timestamped by another node. In this case, the node can preserve the UTC timestamp applied by the Dynamic Driver interface that published the original `ps_lidar_points_msg`.

### 2. Locate the C++ code We can locate the code here by default. ```bash PSYNC_HOME/examples/cpp/GroundPlaneDetection ``` ### 2.1 GroundPlaneDetection.cpp ```cpp #include < PolySyncNode.hpp > #include < PolySyncDataModel.hpp > using namespace std; /class GroundPlaneDetection : public polysync::Node { private: ps_msg_type _messageType; std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints; public: / void initStateEvent() override { _messageType = getMessageTypeByName( "ps_lidar_points_msg" ); registerListener( _messageType ); } void okStateEvent() override { polysync::datamodel::LidarPointsMessage groundPlaneMessage ( *this ); groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() ); groundPlaneMessage.setPoints( _groundPlanePoints ); groundPlaneMessage.publish(); usleep(50); } / virtual void messageEvent( std::shared_ptr< polysync::Message > message ) { using namespace polysync::datamodel; if( std::shared_ptr lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) ) { if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() ) { _groundPlanePoints.clear(); groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() ); std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints(); std::vector< polysync::datamodel::LidarPoint > groundPlanePoints; std::array< float, 3 > position; for( polysync::datamodel::LidarPoint point : lidarPoints ) { position = point.getPosition(); { _groundPlanePoints.push_back( point ); } } //groundPlaneMessage.setPoints( _groundPlanePoints ); // groundPlaneMessage.publish(); groundPlanePoints.clear(); lidarPoints.clear(); } } } bool pointIsNearTheGround( const std::array< float, 3 > & point ) { return point[0] >= 2.5 and // x is 2.5+ meters from the vehicle origin point[0] < 35 and // x is less than 35 meters from the vehicle origin point[1] > -12 and // y is greater than -12 meters from the vehicle origin (towards the passenger side) point[1] < 12 and // y is less than 12 meters from the vehicle origin (towards the driver side) point[2] > -0.35 and // z is greater than -0.35 meters from the vehicle origin (towards the ground), point[2] < 0.25; // z is less than 0.25 meters from the vehicle origin } }; /int main() { return 0; } ``` ### 2.2 GroundPlaneDetection.cpp explained As we saw above, the `_messageType` is used to hold the integer value for the `ps_lidar_points_msg`. `_groundPlanePoints` is used to hold the subset of LiDAR points that represent the ground plane. ```cpp private: ps_msg_type _messageType; std::vector< polysync::datamodel::LidarPoint > _groundPlanePoints; ``` You will call the following functions─almost always in this order─by a node that's publishing messages to the bus. After creating an instance of the `ps_lidar_points_msg`, the payload is set. In this instance the ground plane points vector. Please note that the message header timestamp is set immediately before the publish message function. ```cpp void okStateEvent() override { polysync::datamodel::LidarPointsMessage groundPlaneMessage( *this ); groundPlaneMessage.setPoints( _groundPlanePoints ); groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() ); groundPlaneMessage.publish(); usleep(50); } ``` Within the `messageEvent`, you will promote the base class message to a `LidarPointsMessage`. It's important that the node filters out any message that it publishes to the bus. Once the node receives a valid, new LiDAR points message it can clear out the existing vector of LiDAR ground points. ```cpp if( std::shared_ptr lidarPointsMessage = getSubclass< LidarPointsMessage >( message ) ) { // Filter out this nodes own messages if( lidarPointsMessage->getHeaderSrcGuid() != getGUID() ) { _groundPlanePoints.clear(); ``` Each time the node receives a LiDAR point message, it creates another instance of the LiDAR point message to hold the ground plane points. The original message and ground plane points message will have the same message start/end scan timestamps, which allows other node(s) to correlate the original and ground plane messages for further processing. This code block shows how to create local containers for the incoming message, and iterate over the message payload in the `for` loop. ```cpp LidarPointsMessage groundPlaneMessage ( *this ); groundPlaneMessage.setHeaderTimestamp( polysync::getTimestamp() ); // Get the entire LiDAR point cloud from the incoming message std::vector< polysync::datamodel::LidarPoint > lidarPoints = lidarPointsMessage->getPoints(); // Create a container that will hold all ground plane points that are found in the nodes processing std::vector< polysync::datamodel::LidarPoint > groundPlanePoints; // Create a container to hold a single point as the node iterates over the full point cloud std::array< float, 3 > position; for( polysync::datamodel::LidarPoint point : lidarPoints ) { // Get the x/y/z position for this point in the point cloud position = point.getPosition(); if( pointIsNearTheGround( position ) ) { // This point is close the ground, place it in our point vector _groundPlanePoints.push_back( point ); } } ``` Since the incoming data has been transformed to a vehicle centered reference coordinate frame by default, we can use the following simple filtering techniques to determine which points in the LiDAR cloud are **ground points**. ```cpp bool pointIsNearTheGround( const std::array< float, 3 > & point ) { // The vehicle origin is at the center of the rear axle, on the ground // Incoming LiDAR point messages have been corrected for sensor mount position already return point[0] >= 2.5 and // x is 2.5+ meters from the vehicle origin point[0] < 35 and // x is less than 35 meters from the vehicle origin point[1] > -12 and // y is greater than -12 meters from the vehicle origin (towards the passenger side) point[1] < 12 and // y is less than 12 meters from the vehicle origin (towards the driver side) point[2] > -0.35 and // z is greater than -0.35 meters from the vehicle origin (towards the ground), // this compensates for vehicle pitch as the vehicle drives point[2] < 0.25; // z is less than 0.25 meters from the vehicle origin } ``` ### 3. Build and run ground plane node The node exists in `polysync/examples/cpp/GroundPlaneDetection` by default. You will build the node using `cmake` and `make`. ```bash $ cd $PSYNC_HOME/examples/cpp/GroundPlaneDetection` $ cmake . && make $ ./polysync-ground-plane-detection-cpp ``` The node will quietly wait until LiDAR data is received in the `messageEvent`. Since this node requires LiDAR data as an input, it won't do anything unless LiDAR data─`ps_lidar_points_msg`─is being published to the bus by another node. The PolySync development lifecycle uses the powerful replay tools to recreate real-world scenarios on the bench. Using PolySync Studio, which leverages the Record & Replay API, you can command active nodes to replay data from a logfile and publish high-level messages to the bus. This allows for the development of algorithms that subscribe to messages and process the data contained within the messages in real-time. ### 4. Replaying data In order to begin replaying data, you will need to ensure the network is setup by setting the PolySync IP address to the loopback interface. Next, you will open the [term id="1942"]SDF Configurator[/term] to make sure the network configuration is valid. If a mismatch is detected, the SDF Configurator will open a [term id="1986"]host setup wizard[/term]. ```bash $ polysync-manager -s 127.0.0.1 $ polysync-sdf-configurator ``` With a valid host configuration, you can instruct the PolySync [term id="1969"]manager[/term]to spawn all nodes that are defined on this host and enabled in the SDF. ```bash $ polysync-manager -n -w ``` Now is the time to start PolySync [term id="1967"]Studio[/term] in order to visualize the runtime status of our nodes, and eventually to visualize the LiDAR point data. The [inline id="924"]System Hierarchy[/inline] plugin can be opened from the plugin launcher on the right-hand panel to ensure all nodes are in the "OK" state. ```bash $ polysync-studio ``` Next we select the replay tab from the right-hand panel, and select a replay session for playback by double-clicking. The default session is **1000**. The play button becomes solid black when nodes are ready for playback. ### 5. Visualizing data The data can be visualized in Studio's 3D View and the OpenGL based Viewer Lite example node. Open the **3D View** plugin from the plugin launcher. ### 5.1 Viewer Lite Viewer Lite is an OpenGL 2D visualizer node that allows us to see primitive data types (LiDAR points, objects, and RADAR targets) being transmitted using the PolySync bus and messages. To run PolySync Viewer Lite, use the command: ```bash $ polysync-viewer-lite ``` ### Conclusion Congratulations! You have successfully implemented perception algorithms and walked through using logfile data as an algorithm development acceleration tool.