laser_scan_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <OgreSceneNode.h>
31 #include <OgreSceneManager.h>
32 
33 #include <ros/time.h>
34 
36 
38 #include "rviz/display_context.h"
39 #include "rviz/frame_manager.h"
42 #include "rviz/validate_floats.h"
43 
44 #include "laser_scan_display.h"
45 
46 namespace rviz
47 {
48 
50  : point_cloud_common_( new PointCloudCommon( this ))
51  , projector_( new laser_geometry::LaserProjection() )
52 {
53  queue_size_property_ = new IntProperty( "Queue Size", 10,
54  "Advanced: set the size of the incoming LaserScan message queue. "
55  " Increasing this is useful if your incoming TF data is delayed significantly "
56  "from your LaserScan data, but it can greatly increase memory usage if the messages are big.",
57  this, SLOT( updateQueueSize() ));
58 
59  // PointCloudCommon sets up a callback queue with a thread for each
60  // instance. Use that for processing incoming messages.
62 }
63 
65 {
66  delete point_cloud_common_;
67  delete projector_;
68 }
69 
71 {
74 }
75 
77 {
79 }
80 
81 void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& scan )
82 {
83  sensor_msgs::PointCloudPtr cloud( new sensor_msgs::PointCloud );
84 
85  std::string frame_id = scan->header.frame_id;
86 
87  // Compute tolerance necessary for this scan
88  ros::Duration tolerance(scan->time_increment * scan->ranges.size());
89  if (tolerance > filter_tolerance_)
90  {
91  filter_tolerance_ = tolerance;
93  }
94 
95  try
96  {
99  }
100  catch (tf::TransformException& e)
101  {
102  ROS_DEBUG( "LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance should now be set on our tf::MessageFilter).",
103  qPrintable( getName() ), e.what() );
104  return;
105  }
106 
108 }
109 
110 void LaserScanDisplay::update( float wall_dt, float ros_dt )
111 {
112  point_cloud_common_->update( wall_dt, ros_dt );
113 }
114 
116 {
117  MFDClass::reset();
119 }
120 
121 } // namespace rviz
122 
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
laser_geometry::LaserProjection * projector_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
tf::MessageFilter< sensor_msgs::LaserScan > * tf_filter_
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:38
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
Displays a point cloud of type sensor_msgs::PointCloud.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
virtual void setQueueSize(uint32_t new_queue_size)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
virtual void reset()
Called to tell the display to clear its state.
IntProperty * queue_size_property_
Visualizes a laser scan, received as a sensor_msgs::LaserScan.
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
ros::Duration filter_tolerance_
virtual void processMessage(const sensor_msgs::LaserScanConstPtr &scan)
Process a single message. Overridden from MessageFilterDisplay.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void setTolerance(const ros::Duration &tolerance)
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:159
PointCloudCommon * point_cloud_common_
void update(float wall_dt, float ros_dt)
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41