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_cloud_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/frame_manager.h"
00035
00036 #include <ros/time.h>
00037 #include "rviz/ogre_helpers/point_cloud.h"
00038
00039 #include <tf/transform_listener.h>
00040
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreSceneManager.h>
00043
00044 namespace rviz
00045 {
00046
00047 PointCloudDisplay::PointCloudDisplay()
00048 : PointCloudBase()
00049 , tf_filter_( 0 )
00050 {
00051 }
00052
00053 PointCloudDisplay::~PointCloudDisplay()
00054 {
00055 unsubscribe();
00056 tf_filter_->clear();
00057 delete tf_filter_;
00058 }
00059
00060 void PointCloudDisplay::onInitialize()
00061 {
00062 PointCloudBase::onInitialize();
00063 tf_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud>( *vis_manager_->getTFClient(), "", 10, threaded_nh_ );
00064 tf_filter_->connectInput(sub_);
00065 tf_filter_->registerCallback(&PointCloudDisplay::incomingCloudCallback, this);
00066 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00067 }
00068
00069 void PointCloudDisplay::setQueueSize( int size )
00070 {
00071 if( size != (int) tf_filter_->getQueueSize() )
00072 {
00073 tf_filter_->setQueueSize( (uint32_t) size );
00074 propertyChanged( queue_size_property_ );
00075 }
00076 }
00077
00078 int PointCloudDisplay::getQueueSize()
00079 {
00080 return (int) tf_filter_->getQueueSize();
00081 }
00082
00083 void PointCloudDisplay::setTopic( const std::string& topic )
00084 {
00085 unsubscribe();
00086 topic_ = topic;
00087 reset();
00088 subscribe();
00089
00090 propertyChanged(topic_property_);
00091
00092 causeRender();
00093 }
00094
00095 void PointCloudDisplay::onEnable()
00096 {
00097 PointCloudBase::onEnable();
00098
00099 subscribe();
00100 }
00101
00102 void PointCloudDisplay::onDisable()
00103 {
00104 unsubscribe();
00105 tf_filter_->clear();
00106
00107 PointCloudBase::onDisable();
00108 }
00109
00110 void PointCloudDisplay::subscribe()
00111 {
00112 if ( !isEnabled() )
00113 {
00114 return;
00115 }
00116
00117 try
00118 {
00119 sub_.subscribe(threaded_nh_, topic_, 2);
00120 setStatus(status_levels::Ok, "Topic", "OK");
00121 }
00122 catch (ros::Exception& e)
00123 {
00124 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00125 }
00126 }
00127
00128 void PointCloudDisplay::unsubscribe()
00129 {
00130 sub_.unsubscribe();
00131 }
00132
00133 void PointCloudDisplay::incomingCloudCallback(const sensor_msgs::PointCloudConstPtr& cloud)
00134 {
00135 addMessage(cloud);
00136 }
00137
00138 void PointCloudDisplay::fixedFrameChanged()
00139 {
00140 tf_filter_->setTargetFrame( fixed_frame_ );
00141
00142 PointCloudBase::fixedFrameChanged();
00143 }
00144
00145 void PointCloudDisplay::createProperties()
00146 {
00147 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PointCloudDisplay::getTopic, this ),
00148 boost::bind( &PointCloudDisplay::setTopic, this, _1 ), parent_category_, this );
00149 setPropertyHelpText(topic_property_, "sensor_msgs::PointCloud topic to subscribe to.");
00150 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00151 topic_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::PointCloud>());
00152
00153 queue_size_property_ = property_manager_->createProperty<IntProperty>( "Queue Size", property_prefix_,
00154 boost::bind( &PointCloudDisplay::getQueueSize, this ),
00155 boost::bind( &PointCloudDisplay::setQueueSize, this, _1 ),
00156 parent_category_, this );
00157 setPropertyHelpText( queue_size_property_, "Advanced: set the size of the incoming PointCloud message queue. Increasing this is useful if your incoming TF data is delayed significantly from your PointCloud data, but it can greatly increase memory usage if the messages are big." );
00158
00159 PointCloudBase::createProperties();
00160 }
00161
00162 }