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 {
49  : point_cloud_common_(new PointCloudCommon(this)), projector_(new laser_geometry::LaserProjection())
50 {
52  "Queue Size", 10,
53  "Advanced: set the size of the incoming LaserScan message queue. "
54  " Increasing this is useful if your incoming TF data is delayed significantly "
55  "from your LaserScan data, but it can greatly increase memory usage if the messages are big.",
56  this, SLOT(updateQueueSize()));
57 }
58 
60 {
62  delete point_cloud_common_;
63  delete projector_;
64 }
65 
67 {
68  // Use the threaded queue for processing of incoming messages
70 
73 }
74 
76 {
78 }
79 
80 void LaserScanDisplay::processMessage(const sensor_msgs::LaserScanConstPtr& scan)
81 {
82  sensor_msgs::PointCloudPtr cloud(new sensor_msgs::PointCloud);
83 
84  std::string frame_id = scan->header.frame_id;
85 
86  // Compute tolerance necessary for this scan
87  ros::Duration tolerance(scan->time_increment * scan->ranges.size());
88  if (tolerance > filter_tolerance_)
89  {
90  filter_tolerance_ = tolerance;
92  }
93 
94  try
95  {
96 // TODO(wjwwood): remove this and use tf2 interface instead
97 #ifndef _WIN32
98 #pragma GCC diagnostic push
99 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
100 #endif
101 
102  auto tf_client = context_->getTFClient();
103 
104 #ifndef _WIN32
105 #pragma GCC diagnostic pop
106 #endif
107  projector_->transformLaserScanToPointCloud(fixed_frame_.toStdString(), *scan, *cloud, *tf_client,
109  }
110  catch (tf::TransformException& e)
111  {
112  ROS_DEBUG("LaserScan [%s]: failed to transform scan: %s. This message should not repeat (tolerance "
113  "should now be set on our tf::MessageFilter).",
114  qPrintable(getName()), e.what());
115  return;
116  }
117 
119 }
120 
121 void LaserScanDisplay::update(float wall_dt, float ros_dt)
122 {
123  point_cloud_common_->update(wall_dt, ros_dt);
124 }
125 
127 {
128  MFDClass::reset();
130 }
131 
132 } // namespace rviz
133 
tf2_ros::MessageFilter< sensor_msgs::LaserScan > * tf_filter_
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:287
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:300
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
void processMessage(const sensor_msgs::LaserScanConstPtr &scan) override
Process a single message. Overridden from MessageFilterDisplay.
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
Displays a point cloud of type sensor_msgs::PointCloud.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
void setTolerance(const ros::Duration &tolerance)
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
virtual void setQueueSize(uint32_t new_queue_size)
IntProperty * queue_size_property_
Visualizes a laser scan, received as a sensor_msgs::LaserScan.
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
void reset() override
Called to tell the display to clear its state.
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_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void onInitialize() override
Do initialization. Overridden from MessageFilterDisplay.
PointCloudCommon * point_cloud_common_
virtual ros::CallbackQueueInterface * getThreadedQueue()=0
Return a CallbackQueue using a different thread than the main GUI one.
void update(float wall_dt, float ros_dt)
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Aug 15 2022 02:05:16