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 "laser_scan_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 "rviz/ogre_helpers/point_cloud.h"
00037
00038 #include <tf/transform_listener.h>
00039 #include <sensor_msgs/PointCloud.h>
00040 #include <laser_geometry/laser_geometry.h>
00041
00042 #include <OGRE/OgreSceneNode.h>
00043 #include <OGRE/OgreSceneManager.h>
00044
00045 namespace rviz
00046 {
00047
00048 LaserScanDisplay::LaserScanDisplay()
00049 : PointCloudBase()
00050 {
00051 }
00052
00053 LaserScanDisplay::~LaserScanDisplay()
00054 {
00055 unsubscribe();
00056 tf_filter_->clear();
00057 delete projector_;
00058 delete tf_filter_;
00059 }
00060
00061 void LaserScanDisplay::onInitialize()
00062 {
00063 PointCloudBase::onInitialize();
00064 tf_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*vis_manager_->getTFClient(), "", 10, threaded_nh_);
00065 projector_ = new laser_geometry::LaserProjection();
00066
00067 tf_filter_->connectInput(sub_);
00068 tf_filter_->registerCallback(boost::bind(&LaserScanDisplay::incomingScanCallback, this, _1));
00069 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00070 }
00071
00072 void LaserScanDisplay::setQueueSize( int size )
00073 {
00074 if( size != (int) tf_filter_->getQueueSize() )
00075 {
00076 tf_filter_->setQueueSize( (uint32_t) size );
00077 propertyChanged( queue_size_property_ );
00078 }
00079 }
00080
00081 int LaserScanDisplay::getQueueSize()
00082 {
00083 return (int) tf_filter_->getQueueSize();
00084 }
00085
00086 void LaserScanDisplay::setTopic( const std::string& topic )
00087 {
00088 unsubscribe();
00089
00090 topic_ = topic;
00091 reset();
00092
00093 subscribe();
00094
00095 propertyChanged(topic_property_);
00096
00097 causeRender();
00098 }
00099
00100
00101 void LaserScanDisplay::onEnable()
00102 {
00103 PointCloudBase::onEnable();
00104
00105 subscribe();
00106 }
00107
00108 void LaserScanDisplay::onDisable()
00109 {
00110 unsubscribe();
00111
00112 PointCloudBase::onDisable();
00113 }
00114
00115 void LaserScanDisplay::subscribe()
00116 {
00117 if ( !isEnabled() )
00118 {
00119 return;
00120 }
00121
00122 try
00123 {
00124 sub_.subscribe(threaded_nh_, topic_, 2);
00125 setStatus(status_levels::Ok, "Topic", "OK");
00126 }
00127 catch (ros::Exception& e)
00128 {
00129 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00130 }
00131 }
00132
00133 void LaserScanDisplay::unsubscribe()
00134 {
00135 sub_.unsubscribe();
00136 tf_filter_->clear();
00137 }
00138
00139
00140 void LaserScanDisplay::incomingScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00141 {
00142 sensor_msgs::PointCloudPtr cloud(new sensor_msgs::PointCloud);
00143
00144 std::string frame_id = scan->header.frame_id;
00145
00146
00147 ros::Duration tolerance(scan->time_increment * scan->ranges.size());
00148 if (tolerance > filter_tolerance_)
00149 {
00150 filter_tolerance_ = tolerance;
00151 tf_filter_->setTolerance(filter_tolerance_);
00152 }
00153
00154 try
00155 {
00156 projector_->transformLaserScanToPointCloud(fixed_frame_, *scan, *cloud , *vis_manager_->getTFClient(), laser_geometry::channel_option::Intensity);
00157 }
00158 catch (tf::TransformException& e)
00159 {
00160 ROS_DEBUG("LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance should now be set on our tf::MessageFilter).", name_.c_str(), e.what());
00161 return;
00162 }
00163 addMessage(cloud);
00164 }
00165
00166 void LaserScanDisplay::fixedFrameChanged()
00167 {
00168 tf_filter_->setTargetFrame(fixed_frame_);
00169
00170 PointCloudBase::fixedFrameChanged();
00171 }
00172
00173 void LaserScanDisplay::createProperties()
00174 {
00175 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &LaserScanDisplay::getTopic, this ),
00176 boost::bind( &LaserScanDisplay::setTopic, this, _1 ), parent_category_, this );
00177 setPropertyHelpText(topic_property_, "sensor_msgs::LaserScan topic to subscribe to.");
00178 ROSTopicStringPropertyPtr str_prop = topic_property_.lock();
00179 str_prop->addLegacyName("Scan Topic");
00180 str_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::LaserScan>());
00181
00182 queue_size_property_ = property_manager_->createProperty<IntProperty>( "Queue Size", property_prefix_,
00183 boost::bind( &LaserScanDisplay::getQueueSize, this ),
00184 boost::bind( &LaserScanDisplay::setQueueSize, this, _1 ),
00185 parent_category_, this );
00186 setPropertyHelpText( queue_size_property_, "Advanced: set the size of the incoming LaserScan message queue. Increasing this is useful if your incoming TF data is delayed significantly from your LaserScan data, but it can greatly increase memory usage if the messages are big." );
00187
00188 PointCloudBase::createProperties();
00189 }
00190
00191 }