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 "point_cloud2_display.h"
00031 #include "point_cloud_transformers.h"
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/properties/property_manager.h"
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/validate_floats.h"
00037
00038 #include <ros/time.h>
00039 #include "rviz/ogre_helpers/point_cloud.h"
00040
00041 #include <tf/transform_listener.h>
00042
00043 #include <OGRE/OgreSceneNode.h>
00044 #include <OGRE/OgreSceneManager.h>
00045
00046 namespace rviz
00047 {
00048
00049 PointCloud2Display::PointCloud2Display()
00050 : PointCloudBase()
00051 , tf_filter_( 0 )
00052 {
00053 }
00054
00055 PointCloud2Display::~PointCloud2Display()
00056 {
00057 unsubscribe();
00058 tf_filter_->clear();
00059 delete tf_filter_;
00060 }
00061
00062 void PointCloud2Display::onInitialize()
00063 {
00064 PointCloudBase::onInitialize();
00065 tf_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*vis_manager_->getTFClient(), "", 10, threaded_nh_);
00066 tf_filter_->connectInput(sub_);
00067 tf_filter_->registerCallback(&PointCloud2Display::incomingCloudCallback, this);
00068 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00069 }
00070
00071 void PointCloud2Display::setQueueSize( int size )
00072 {
00073 if( size != (int) tf_filter_->getQueueSize() )
00074 {
00075 tf_filter_->setQueueSize( (uint32_t) size );
00076 propertyChanged( queue_size_property_ );
00077 }
00078 }
00079
00080 int PointCloud2Display::getQueueSize()
00081 {
00082 return (int) tf_filter_->getQueueSize();
00083 }
00084
00085 void PointCloud2Display::setTopic( const std::string& topic )
00086 {
00087 unsubscribe();
00088 topic_ = topic;
00089 reset();
00090 subscribe();
00091
00092 propertyChanged(topic_property_);
00093
00094 causeRender();
00095 }
00096
00097 void PointCloud2Display::onEnable()
00098 {
00099 PointCloudBase::onEnable();
00100
00101 subscribe();
00102 }
00103
00104 void PointCloud2Display::onDisable()
00105 {
00106 unsubscribe();
00107 tf_filter_->clear();
00108
00109 PointCloudBase::onDisable();
00110 }
00111
00112 void PointCloud2Display::subscribe()
00113 {
00114 if ( !isEnabled() )
00115 {
00116 return;
00117 }
00118
00119 try
00120 {
00121 sub_.subscribe(threaded_nh_, topic_, 2);
00122 setStatus(status_levels::Ok, "Topic", "OK");
00123 }
00124 catch (ros::Exception& e)
00125 {
00126 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00127 }
00128 }
00129
00130 void PointCloud2Display::unsubscribe()
00131 {
00132 sub_.unsubscribe();
00133 }
00134
00135 void PointCloud2Display::incomingCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud)
00136 {
00137
00138
00139
00140 sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
00141 int32_t xi = findChannelIndex(cloud, "x");
00142 int32_t yi = findChannelIndex(cloud, "y");
00143 int32_t zi = findChannelIndex(cloud, "z");
00144
00145 if (xi == -1 || yi == -1 || zi == -1)
00146 {
00147 return;
00148 }
00149
00150 const uint32_t xoff = cloud->fields[xi].offset;
00151 const uint32_t yoff = cloud->fields[yi].offset;
00152 const uint32_t zoff = cloud->fields[zi].offset;
00153 const uint32_t point_step = cloud->point_step;
00154 const size_t point_count = cloud->width * cloud->height;
00155 filtered->data.resize(cloud->data.size());
00156 if (point_count == 0)
00157 {
00158 return;
00159 }
00160
00161 uint32_t output_count = 0;
00162 const uint8_t* ptr = &cloud->data.front();
00163 for (size_t i = 0; i < point_count; ++i)
00164 {
00165 float x = *reinterpret_cast<const float*>(ptr + xoff);
00166 float y = *reinterpret_cast<const float*>(ptr + yoff);
00167 float z = *reinterpret_cast<const float*>(ptr + zoff);
00168 if (validateFloats(Ogre::Vector3(x, y, z)))
00169 {
00170 memcpy(&filtered->data.front() + (output_count * point_step), ptr, point_step);
00171 ++output_count;
00172 }
00173
00174 ptr += point_step;
00175 }
00176
00177 filtered->header = cloud->header;
00178 filtered->fields = cloud->fields;
00179 filtered->data.resize(output_count * point_step);
00180 filtered->height = 1;
00181 filtered->width = output_count;
00182 filtered->is_bigendian = cloud->is_bigendian;
00183 filtered->point_step = point_step;
00184 filtered->row_step = output_count;
00185
00186 addMessage(filtered);
00187 }
00188
00189 void PointCloud2Display::fixedFrameChanged()
00190 {
00191 tf_filter_->setTargetFrame( fixed_frame_ );
00192
00193 PointCloudBase::fixedFrameChanged();
00194 }
00195
00196 void PointCloud2Display::createProperties()
00197 {
00198 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_,
00199 boost::bind( &PointCloud2Display::getTopic, this ),
00200 boost::bind( &PointCloud2Display::setTopic, this, _1 ),
00201 parent_category_, this );
00202 setPropertyHelpText(topic_property_, "sensor_msgs::PointCloud2 topic to subscribe to.");
00203 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00204 topic_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::PointCloud2>());
00205
00206 queue_size_property_ = property_manager_->createProperty<IntProperty>( "Queue Size", property_prefix_,
00207 boost::bind( &PointCloud2Display::getQueueSize, this ),
00208 boost::bind( &PointCloud2Display::setQueueSize, this, _1 ),
00209 parent_category_, this );
00210 setPropertyHelpText( queue_size_property_, "Advanced: set the size of the incoming PointCloud2 message queue. Increasing this is useful if your incoming TF data is delayed significantly from your PointCloud2 data, but it can greatly increase memory usage if the messages are big." );
00211
00212 PointCloudBase::createProperties();
00213 }
00214
00215 }