Saving Point Cloud Data Through UDP

Now that we have explained the basics of socket programming in C++ for a UDP communication, it is time to look at an important piece of our code which uses a re-ceive function iteratively to read and save the point cloud data. This code has been pasted from the closed loop phase. The open loop phase is exactly the same, but without the while loop to run only once. The reading and saving works as follows;

rst a single double which is the control signal is read in. This signal is described in detail in the next section, but for now it is enough to know that this is a condition signal to control the read and save of data. If this value is one we proceed to read one row of the point cloud data, and we do not read if otherwise.

while ( true )

{

recvfrom ( control_signal_socket , control_signal_value , sizeof ( control_signal_value ) , 0 ,

( sockaddr *)& c o n t r o l _ s i g n a l _ c l i e n t ,

&c o n t r o l _ s i g n a l _ c l i e n t _ s i z e ) ; i f ( control_signal_value [ 0 ] == 1)

{

recvfrom ( pointcloud_read_socket , &row_package1 , sizeof ( row_package1 ) , 0 ,

( sockaddr *)& pointcloud_read_client ,

&pointcloud_read_client_size ) ; udpVectorData1 . push_back ( row_package1 ) ; number_of_rows1 ++;

}

i f ( control_signal_value [ 0 ] == 0 & number_of_rows1!= 0) {

cout << "number o f rows : " << number_of_rows1 << endl ; break ;

} }

Note that both receive functions are in blocking mode, so the rst function which is reading the control signal, waits there until a data is available. This signal is coming every 5ms, so if we are lucky not to loose it, this waiting should not take more than that. If this value is equal to one, another receive function is called. This function is in blocking mode as well, but there is a signicantly important dierence in between. The control signal is sent to this socket as unicast, and unicast data will be placed in a structure so called a queue. Once read the package will be removed from the queue and there will not be any data available before another 5ms. In contrast the point cloud data are sent as broadcast and they will not be removed when read. In fact there is no meaning for a queue for broadcasting, since every user should be able to log and receive the broadcast data. This means that even though these data are coming every 20ms, but the second receive will not need to wait this time to read a data, the previous data is always available in between these 20 milliseconds and will not be removed even if it is read several times. With a simple division it can be seen that we will be reading the same data four times repeatedly before the new data is available after a 20ms.

This is a good strategy we have implemented to compensate the unreliable nature of UDP. In other words, although the reading process is much faster than20ms, but this is a limitation since no new data will be available before this time. Experience shows that reading only once during this20mswill decrease the chance of receiving the package, and a great number of data were lost. It is notable that this way the reliability of reading data correctly is increased without loosing any time, which is

desirable.

One row of point cloud data consists of 721 double values, consisting of a single

"sweep" value, 180 doubles of "variance", and 180 "point data" each having three values for x, y, and z. We have dened two structures to save in data and to ease accessing of the elements. The rst structure which is called LaserData holds coor-dinate values, and the second structure, UDPData, is a structure of structure and holds the value of sweep, variances and its third element is180of LaserData type, as shown below. An object of type UDPData is then dened and called row_package1 which is basically one row which will be read at each call of receive function. Finally a vector called udpVectorData1 of the type UDPData will hold all the rows to form a complete point cloud, and this is done by the push_back function. In addition an integer number_of_rows1 will decrease by one each time a row is saved in that vector.

struct LaserData

There still is another step to complete. You can see that the while loop which iter-ates and saves data in a vector has a condition of always true, so we need to give it a condition to break. This is satised when both control_signal_value is zero and number_of_rows1 is not zero. This guarantees that the control signal has had a falling edge, meaning it has fallen to zero after a one and saving some data, and not being in zero before saving any points when it has not even reached there. At this point the reading from UDP socket is nished and all data which we were allowed to read during the high period of control signal is saved in a vector, ready to be transformed to a PCL usable format. The relative piece of code which transforms these data to a PCL compatible point cloud form is pasted here. This is a simple iteration to go through the vector and access the coordinates and save them in a pointer called cloud which will be usable by PCL for further processing.

p c l : : PointCloud<p cl : : PointXYZ >:: Ptr cloud (new p cl : : PointCloud<p c l : : PointXYZ >( ) );

int index = 0 ;

cloud −> width = number_of_rows * 180;

cloud −> height = 1 ; cloud −> is_dense = f a l s e ;

cloud −> p o i n t s . r e s i z e ( cloud −> width * cloud −> height ) ; for ( int i = 0 ; i < udpVectorData . s i z e ( ) ; i++)

{

for ( int u = 0 ; u < 180; u++) {

cloud −> p o i n t s [ index+u ] . x = udpVectorData . at ( i ) . data [ u ] . x ; cloud −> p o i n t s [ index+u ] . y = udpVectorData . at ( i ) . data [ u ] . y ; cloud −> p o i n t s [ index+u ] . z = udpVectorData . at ( i ) . data [ u ] . z ; }

index = index + 180;

}

Figure 3.4 summarizes all UDP communication performed in our algorithm in a schematic representation.

Figure 3.4: UDP communication schematic diagram of the system