Mocking Point Clouds in ROS Rviz

Robot Operating System (ROS) is a powerful tool we can use with robot simulations. Simulating a point cloud is one of the things I'm interested in and this blog post is a type of a note to self. Before we start, there are few things we need to get the simulation done.

Ingredients

1. ROS: we need to have ROS installed in my computer. It's pretty straightforward and one can refer the official documentation on installation steps. Make sure ROS is installed and working without any errors by running the following command in a terminal;

$ roscore

2. Rviz: rviz is a part of ROS. Make sure it's available and running, by running the following command in a terminal window;

$ rosrun rviz rviz

You should get a graphical interface looks like the following window

Figure 1. Rviz window

3. Apart from all those above, there are few additional dependencies you would need. I would strongly suggest you use a virtual environment using pyenv to install and run the whole thing as it would leave the system python dependencies intact even if something goes wrong. To create a python virtual environment, execute the following command in a terminal. It will create a directory named pythonenv;

$ virtualenv -p python3 pythonenv

To activate the virtual environment, run the following command and you will see the prompt changed.

$ source pythonenv/bin/activate

Once you're done, just type deactivate and it will bring you out of the python virtual environment.

Package	Installation
rospkg	$ pip install rospkg
yaml	$ pip install pyyaml

Sample Point Cloud

If you just want to try if the method works, I would like to provide you with some resources. In this link, you can find a sample CSV file containing a set of coordinates with their x, y and z locations. You would need to read the file (probably using pandas) and follow the steps in the next section.

The Initial Setup

Now comes the core part of this blog; creating and publishing the actual point cloud message. Point clouds belong to the sensor_msgs package in ROS and we are interested in the PointCloud class. The link would lead you to the official class declaration. This class would need the following properties.

Property	Description
header	contains details related to the overall frame
points	points in the point cloud

Without going on to any trivial details, let's get to actual coding. Find out more about from official documentation. It's much easier.

Open a text editor and add the following imports to the top of the file.

import rospy
from sensor_msgs.msg import PointCloud
from std_msgs.msg import Header
from geometry_msgs.msg import Point32

In a nutshell, rospy is needed to run the ros node that would be publishing the pointcloud message to roscore. Other imports are from ros python package and they would help us creating the sensor message.

Let's initiate the ros node and create a publisher. This publisher will publish 'pointcloud' messages at a rate of 5 as defined by rospy rate.

rospy.init_node('mock_scanner')
scanPublisher = rospy.Publisher('pointcloud', PointCloud, queue_size=50)
r = rospy.Rate(5.0)

Next step is to start publishing. It's not easy as it sounds. We'll have to create a while loop that runs indefinitely until roscore is shutdown. First let me get the code snippet.

n = 1
while not rospy.is_shutdown():
    print('We are publishing')
    laserScan = generateLaserScan(pointList, n, rospy.Time.now())
    scanPublisher.publish(laserScan)
    n += 1
    # Sleep will continue publishing points at a constant speed
    r.sleep()

What this does is, it will run in a while loop until the roscore is terminated. Having said that, it assumes that the roscore is running. If not, it will display a message saying that

Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.

The method generateLaserScan() will take a list of points with each point having three properties as x, y and z and an incrementing variable n and time stamp. The timestamp better be from ROS to make things less complicated. The publisher as we have initiated earlier will publish the pointcloud message returned from generateLaserScan() method and wait/sleep till the next cycle.

The Point Cloud message

Now we come to the preparation stage of the PointCloud message.

def generateLaserScan(pointList, n, t):
    (X, Y, Z) = pointList
    l = len(Z)
    # Define the header block
    scanHeader = Header()
    scanHeader.seq = n
    scanHeader.stamp = t
    scanHeader.frame_id = 'point_frame'
    # Generate point cloud
    points = []
    for i in range(l):
        p = Point32()
        p.x = X[i]
        p.y = Y[i]
        p.z = Z[i]
        points.append(p)
    # Prepare the pointcloud message
    pc = PointCloud()
    pc.header = scanHeader
    pc.points = points
    return pc

This method is responsible for generating the PointCloud message and returning it. Keep a special note to the frame_id in header as we will be needing it to define the map tranformation. The pointList is assumed to have 3 lists each containing the coordinate point of each point in the cloud. Just read the method carefully and refer to the official documentation and you'll get the idea of creating the "PointCloud" object. As an additional note, each x, y and z points need to be floats.

Running the script

Once the source code is ready, execute it as normal python script. Once again I recommend to run it in the python virtual environment. You will need four separate terminal windows or tabs to get the map running. Follow the following steps in that order.

Terminal 1

Run the roscore and start the ROS master. Simply execute the following command.

$ roscore

it will show a result similar to Fig. 2

Figure 2. roscore output

Terminal 2

It's time to run the static transform between the map and the scan frame. Run the following command.

$ rosrun tf static_transform_publisher 0 0 0 0 0 0 map point_frame 100

Static transform publisher will define relative to the world (map frame), where the origin of the point cloud is. Read more about them here at ROS wiki. If you happened to set a different frame_id to the PointCloud message, replace the point_frame with that name. Otherwise rviz will throw an error as shown in Fig. 4

Terminal 3

Run the python script where we have set the ros publisher who is publishing the rostopic "pointcloud" out loud.

$ python mockpointcloud.py

The code snippet I mentioned, would keep on printing a message "We are publishing" if the script executed successfully.

Terminal 4

It's time to start and set up rviz. Run the following command and it will open rviz GUI.

$ rosrun rviz rviz

This would open the rviz window as shown in Fig. 1 and you wouldn't see any point clouds. The reason is we haven't set any point cloud topic in rviz. Follow the steps as shown in Fig. 3 for now.

Figure 3. Setting pointcloud rostopic in rviz

The one in step 3 will be the topic being published by the python script we ran in terminal 3. If for some reason, the static transformation is not there, rviz will show an error as in Fig. 4 and there will be no point cloud maps.

Figure 4. rviz error when static transformation is failing

The command in terminal 2 will fix this error. Of course you wouldn't see this screen if you did everything correctly. Note that the Fixed Frame is "map" and that is what we mentioned as the base frame in terminal 2 static transformation.

Final Result

So, if everything happened smoothly, you would get a pointcloud similar to Fig. 5 and task is complete.

Figure 5. Final result

The following video shows a view around the map and it's much smoother than plotting the point cloud in a scatterplot in matplotlib for an instance.

What to do with this result is up to you. Hope you got everything right and feel free to comment below if you have any issues. Thanks for reading...

Search This Blog

Padmal's BLOG