Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <OgreSceneNode.h>
00031 #include <OgreSceneManager.h>
00032
00033 #include <ros/time.h>
00034
00035 #include "rviz/default_plugin/point_cloud_common.h"
00036 #include "rviz/default_plugin/point_cloud_transformers.h"
00037 #include "rviz/display_context.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/ogre_helpers/point_cloud.h"
00040 #include "rviz/properties/int_property.h"
00041 #include "rviz/validate_floats.h"
00042
00043 #include "point_cloud2_display.h"
00044
00045 namespace rviz
00046 {
00047
00048 PointCloud2Display::PointCloud2Display()
00049 : point_cloud_common_( new PointCloudCommon( this ))
00050 {
00051 queue_size_property_ = new IntProperty( "Queue Size", 10,
00052 "Advanced: set the size of the incoming PointCloud2 message queue. "
00053 " Increasing this is useful if your incoming TF data is delayed significantly "
00054 "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.",
00055 this, SLOT( updateQueueSize() ));
00056
00057
00058
00059 update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() );
00060 }
00061
00062 PointCloud2Display::~PointCloud2Display()
00063 {
00064 delete point_cloud_common_;
00065 }
00066
00067 void PointCloud2Display::onInitialize()
00068 {
00069 MFDClass::onInitialize();
00070 point_cloud_common_->initialize( context_, scene_node_ );
00071 }
00072
00073 void PointCloud2Display::updateQueueSize()
00074 {
00075 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00076 }
00077
00078 void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud )
00079 {
00080
00081
00082
00083 sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
00084 int32_t xi = findChannelIndex(cloud, "x");
00085 int32_t yi = findChannelIndex(cloud, "y");
00086 int32_t zi = findChannelIndex(cloud, "z");
00087
00088 if (xi == -1 || yi == -1 || zi == -1)
00089 {
00090 return;
00091 }
00092
00093 const uint32_t xoff = cloud->fields[xi].offset;
00094 const uint32_t yoff = cloud->fields[yi].offset;
00095 const uint32_t zoff = cloud->fields[zi].offset;
00096 const uint32_t point_step = cloud->point_step;
00097 const size_t point_count = cloud->width * cloud->height;
00098
00099 if( point_count * point_step != cloud->data.size() )
00100 {
00101 std::stringstream ss;
00102 ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width
00103 << ") times height (" << cloud->height << ") times point_step (" << point_step << "). Dropping message.";
00104 setStatusStd( StatusProperty::Error, "Message", ss.str() );
00105 return;
00106 }
00107
00108 filtered->data.resize(cloud->data.size());
00109 if (point_count == 0)
00110 {
00111 return;
00112 }
00113
00114 uint8_t* output_ptr = &filtered->data.front();
00115 const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
00116 size_t points_to_copy = 0;
00117 for (; ptr < ptr_end; ptr += point_step)
00118 {
00119 float x = *reinterpret_cast<const float*>(ptr + xoff);
00120 float y = *reinterpret_cast<const float*>(ptr + yoff);
00121 float z = *reinterpret_cast<const float*>(ptr + zoff);
00122 if (validateFloats(x) && validateFloats(y) && validateFloats(z))
00123 {
00124 if (points_to_copy == 0)
00125 {
00126
00127 ptr_init = ptr;
00128 points_to_copy = 1;
00129 }
00130 else
00131 {
00132 ++points_to_copy;
00133 }
00134 }
00135 else
00136 {
00137 if (points_to_copy)
00138 {
00139
00140 memcpy(output_ptr, ptr_init, point_step*points_to_copy);
00141 output_ptr += point_step*points_to_copy;
00142 points_to_copy = 0;
00143 }
00144 }
00145 }
00146
00147 if (points_to_copy)
00148 {
00149 memcpy(output_ptr, ptr_init, point_step*points_to_copy);
00150 output_ptr += point_step*points_to_copy;
00151 }
00152 uint32_t output_count = (output_ptr - &filtered->data.front()) / point_step;
00153
00154 filtered->header = cloud->header;
00155 filtered->fields = cloud->fields;
00156 filtered->data.resize(output_count * point_step);
00157 filtered->height = 1;
00158 filtered->width = output_count;
00159 filtered->is_bigendian = cloud->is_bigendian;
00160 filtered->point_step = point_step;
00161 filtered->row_step = output_count;
00162
00163 point_cloud_common_->addMessage( filtered );
00164 }
00165
00166
00167 void PointCloud2Display::update( float wall_dt, float ros_dt )
00168 {
00169 point_cloud_common_->update( wall_dt, ros_dt );
00170 }
00171
00172 void PointCloud2Display::reset()
00173 {
00174 MFDClass::reset();
00175 point_cloud_common_->reset();
00176 }
00177
00178 }
00179
00180 #include <pluginlib/class_list_macros.h>
00181 PLUGINLIB_EXPORT_CLASS( rviz::PointCloud2Display, rviz::Display )