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 <laser_geometry/laser_geometry.h>
00036
00037 #include "rviz/default_plugin/point_cloud_common.h"
00038 #include "rviz/display_context.h"
00039 #include "rviz/frame_manager.h"
00040 #include "rviz/ogre_helpers/point_cloud.h"
00041 #include "rviz/properties/int_property.h"
00042 #include "rviz/validate_floats.h"
00043
00044 #include "laser_scan_display.h"
00045
00046 namespace rviz
00047 {
00048
00049 LaserScanDisplay::LaserScanDisplay()
00050 : point_cloud_common_( new PointCloudCommon( this ))
00051 , projector_( new laser_geometry::LaserProjection() )
00052 {
00053 queue_size_property_ = new IntProperty( "Queue Size", 10,
00054 "Advanced: set the size of the incoming LaserScan message queue. "
00055 " Increasing this is useful if your incoming TF data is delayed significantly "
00056 "from your LaserScan data, but it can greatly increase memory usage if the messages are big.",
00057 this, SLOT( updateQueueSize() ));
00058
00059
00060
00061 update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() );
00062 }
00063
00064 LaserScanDisplay::~LaserScanDisplay()
00065 {
00066 delete point_cloud_common_;
00067 delete projector_;
00068 }
00069
00070 void LaserScanDisplay::onInitialize()
00071 {
00072 MFDClass::onInitialize();
00073 point_cloud_common_->initialize( context_, scene_node_ );
00074 }
00075
00076 void LaserScanDisplay::updateQueueSize()
00077 {
00078 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00079 }
00080
00081 void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& scan )
00082 {
00083 sensor_msgs::PointCloudPtr cloud( new sensor_msgs::PointCloud );
00084
00085 std::string frame_id = scan->header.frame_id;
00086
00087
00088 ros::Duration tolerance(scan->time_increment * scan->ranges.size());
00089 if (tolerance > filter_tolerance_)
00090 {
00091 filter_tolerance_ = tolerance;
00092 tf_filter_->setTolerance(filter_tolerance_);
00093 }
00094
00095 try
00096 {
00097 projector_->transformLaserScanToPointCloud( fixed_frame_.toStdString(), *scan, *cloud, *context_->getTFClient(),
00098 laser_geometry::channel_option::Intensity );
00099 }
00100 catch (tf::TransformException& e)
00101 {
00102 ROS_DEBUG( "LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance should now be set on our tf::MessageFilter).",
00103 qPrintable( getName() ), e.what() );
00104 return;
00105 }
00106
00107 point_cloud_common_->addMessage( cloud );
00108 }
00109
00110 void LaserScanDisplay::update( float wall_dt, float ros_dt )
00111 {
00112 point_cloud_common_->update( wall_dt, ros_dt );
00113 }
00114
00115 void LaserScanDisplay::reset()
00116 {
00117 MFDClass::reset();
00118 point_cloud_common_->reset();
00119 }
00120
00121 }
00122
00123 #include <pluginlib/class_list_macros.h>
00124 PLUGINLIB_EXPORT_CLASS( rviz::LaserScanDisplay, rviz::Display )