point_cloud_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 } // namespace rviz


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32