Revision history [back]

It is very likely that rviz chokes up because of the amount of markers. I think each arrow is a separate 3D object. I would recommend using the pcl visualization tool:

http://www.ros.org/wiki/pcl_visualization

You can use the existing PointNormal data type, defined here:

http://www.ros.org/doc/api/pcl/html/point__types_8h_source.html

Since you already have the algorithm to calculate the normals, all you have to do is build a pcl cloud and fill out the fields. Here's the nice part about using pcl - you can set how many normals you want to display - for example, one every 10 or 20 points. This should make it much quicker.