ROS provides packages to allow for SLAM, such as gmapping (from OpenSLAM.org) however these usually require a LaserScan Msg. However no open documentation exists (as of writing) for use of these packages without a laser, while it is very possible to do SLAM with other sensors.
Throughout this documentation, we will be basing it around our sensor, a sonar, however it should provide enough information to allow it to be done in a similar fashion to adapt to most sensors.
Converting sensor readings to a 2D plane:
Requirements:
- A sensor outputting the following:
- * Bearing.
- * Range or a number of readings at different ranges.
The sonar in use, takes a full 360 degree scan and returns a number of samples (bins) at predetermined ranges. The sonar driver outputs both these bits of information, the bearing as a single value (int or float) and the ranges as an array.
Using this information we need to convert the data in to a 2D array representing it from its origin, this is done in our sonarInjector node.
The main points of the code are explained below:
int main(int argc, char **argv)
{
//init this
ros::init(argc, argv, "injectPoints");
//Set up node handle
ros::NodeHandle n;
//Publish an array containing
ros::Publisher pub = n.advertise<std_msgs::Float32MultiArray>("sonarPCarray", 100);
ros::Subscriber sub1 = n.subscribe("sonarBearing", 100, bearingCB);
ros::Subscriber sub2 = n.subscribe("sonarBinsArr", 100, binsArrCB);
//set up variables to be published
std_msgs::Float32MultiArray array;
//set the node to loop 30 times per second.
ros::Rate r(30);
while (ros::ok())
{
//Clear array
array.data.clear();
//Loop for each range reading (bin)
for(int i = 0; i < 90; i ++)
{
pixelPlace(bearing, i, binsArr[i]); //convert the current range, bearing and
//reading to an X,Y,Z and intensity to be published to the pointcloud maker.
//explained below:
array.data.push_back(imgx); //x
array.data.push_back(imgy); //y
array.data.push_back(0.0); //z
array.data.push_back(imgz); //intensity
//Publish array
pub.publish(array);
}
//Let the world know
ros::spinOnce();
//Added a delay so not to spam
r.sleep();
}
}
The main section of the code above is pretty simple, it sets up subscriptions to the sonars readings and creates a std_msg to publish an array to be retrieved by the node used to generate the point cloud (sonarPointCloud).
The most important part is the pixelPlace function, which is used to convert a reading at a bearing and range to a point in a 2D grid.
Essentially it performs trigonometry to generate this, through:
x = d * cos(θ)
y = d * sin(θ)
Where:
- d is distance (in our case the range this reading is at).
- θ is the bearing of the reading from the sensor (in radians).
- x is the position in the x axis of a 2D plane
- y is the position in the y axis of a 2D plane
However, depending on the reading of the angle some of the angles require rotating in order for this method to work, hence the 4 if statements in the code below, corresponding to the four quadrants of the circle shown above.
void pixelPlace( float theta, unsigned int distance, unsigned int depth )
{
float x, y;
theta /= 17.7777778; //convert from the sonars step angle to degrees.
if( theta > 0 && theta < 90 )
{
theta = 90.0 - theta;
x = (float)distance * cos( theta *PI/180);
y = (float)distance * sin( theta *PI/180);
imgx = x;
imgy = (y * -1);
}
else if( theta > 91 && theta < 180 )
{
y = (float)distance * cos(theta *PI/180);
x = (float)distance * sin(theta *PI/180);
imgx = x;
imgy = (y * -1);
}
else if( theta > 181 && theta < 270 )
{
theta = 90.0 - theta;
x = (float)distance * cos(theta *PI/180);
y = (float)distance * sin(theta *PI/180);
imgx = x;
imgy = (y * -1);
}
else
{
y = (float)distance * cos(theta *PI/180);
x = (float)distance * sin(theta *PI/180);
imgx = x;
imgy = (y * -1);
}
imgz = float(depth);
}
Once the data has been converted in this manor it can be fed in to the pointcloud2, we opted to do this as a separate node however it could easily be combined with the above.
Converting the 2D plane to a point cloud:
The second node we needed was made to take the array passed out by the injector, reading it in while converting it to a pointcloud. It also thresholds the data and plots only the first hit above that threshold, for the reason in the note below.
Note:
Initially we attempted to plot all the data from all the readings in the pointcloud, while this works for the pointcloud the later stage of converting it to a laserscan message (using pointcloud_to_laserscan) took the first reading as the laser reading. This is because the pointcloud by the node takes any point as a hit and lasers readings are binary (as in, at range r they are either a hit or not).
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float32MultiArray.h"
#define MAX_POINTS 20000 //Maximum points to display
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud; //create a pointcloud using XYZ and I(ntensity)
float Arr[541];
void arrayCallback(const std_msgs::Float32MultiArray::ConstPtr& array);
int main( int argc, char** argv )
{
ros::init(argc, argv, "pointCloud");
ros::NodeHandle n;
//Publish
ros::Publisher pub = n.advertise<PointCloud> ("sonarPC", 100);
//Subscribe
ros::Subscriber sub3 = n.subscribe("sonarPCarray", 100, arrayCallback);
//Set loop rate (times per second)
ros::Rate r(30);
//Create a pointcloud message.
PointCloud::Ptr msg (new PointCloud);
msg->header.frame_id = "base_sonar";
msg->height = 1;
msg->width = MAX_POINTS;
msg->points.resize (msg->width * msg->height);
ros::spinOnce();
int counter = 0;
while (ros::ok())
{
for(int i = 0; i < 90; i ++)
{
//threshold the data, only display if reading above 90.0
// also ignore the first 10, as it's noise from the sonar.
if( (i>=10) && (Arr[3+(i*4)] >= 90.0) )
{
msg->points[counter].x = Arr[0+(i*4)]; //X point
msg->points[counter].y = Arr[2+(i*4)]; //Y point (unused)
msg->points[counter].z = Arr[1+(i*4)]; //Z point
msg->points[counter].intensity = Arr[3+(i*4)]; //sensor reading.
msg->header.stamp = ros::Time::now ();
pub.publish(msg);
counter ++;
i = 90;
}
}
ros::spinOnce();
r.sleep();
//Limit the total number of points in the point cloud, can really kill rviz if too high.
if(counter >= MAX_POINTS)
{
counter = 0;
}
}
}
/*************************************************
** Returns the array from sonarInjector **
*************************************************/
void arrayCallback(const std_msgs::Float32MultiArray::ConstPtr& array)
{
for(std::vector<float>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
{
Arr[i] = *it;
i++;
}
return;
}
With this code running, you should be able to open rviz rosrun rviz rviz and display the data on the screen. You'll notice that the data is actually being plotted in the X and Z plane, rather than what you might expect (XY). This is because the pointcloud_to_laserscan node is looking on these planes.
Converting the point cloud to a laserscan (with pointcloud_to_laserscan):
Get pointcloud_to_laserscan, place the nodes folder in your ros directory and make it.
With all the nodes compiled, the following launch file will allow you to launch them all, including pointcloud_to_laserscan (which is actually a nodelet and therefore requires the nodelet manager).
Make sure to set up the output_frame_id as the frame you want the laserscan message to be published on, the cloud parameter is the message it looks for the pointcloud and scan is the name of the published laserscan.
<param name="output_frame_id" value="/base_sonar"/>
<remap from="cloud" to="sonarPC"/>
<remap from="scan" to="sonarLaser"/>
Obviously make sure to change the package from sonar to the name of the driver outputting the bearing, range and values and the injector and pointcloud nodes to whatever yours are named.
<launch>
<!-- DISPLAY NAME NODE NAME RUN NAME ARGUMENTS -->
<node name="Scanner" pkg="sonar" type="sonar" />
<node name="Injector" pkg="sonarInjector" type="sonarInjector" />
<node name="Pointcloud" pkg="sonarPointCloud" type="sonarPointCloud" />
<node pkg="nodelet" type="nodelet" name="sonarManager" output="screen" respawn="true" args="manager"/>
<!-- fake laser -->
<node pkg="nodelet" type="nodelet" name="sonarToLaser" args="load pointcloud_to_laserscan/CloudToScan sonarManager">
<param name="output_frame_id" value="/base_sonar"/>
<remap from="cloud" to="sonarPC"/>
<remap from="scan" to="sonarLaser"/>
</node>
</launch>
