point_cloud2_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, 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 
37 #include "rviz/display_context.h"
38 #include "rviz/frame_manager.h"
41 #include "rviz/validate_floats.h"
42 
43 #include "point_cloud2_display.h"
44 
45 namespace rviz
46 {
47 
49  : point_cloud_common_( new PointCloudCommon( this ))
50 {
51  queue_size_property_ = new IntProperty( "Queue Size", 10,
52  "Advanced: set the size of the incoming PointCloud2 message queue. "
53  " Increasing this is useful if your incoming TF data is delayed significantly "
54  "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.",
55  this, SLOT( updateQueueSize() ));
56 
57  // PointCloudCommon sets up a callback queue with a thread for each
58  // instance. Use that for processing incoming messages.
60 }
61 
63 {
64  delete point_cloud_common_;
65 }
66 
68 {
71 }
72 
74 {
76 }
77 
78 void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud )
79 {
80  // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase
81  // will get their points put off in lala land, but it means they still do get processed/rendered
82  // which can be a big performance hit
83  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
84  int32_t xi = findChannelIndex(cloud, "x");
85  int32_t yi = findChannelIndex(cloud, "y");
86  int32_t zi = findChannelIndex(cloud, "z");
87 
88  if (xi == -1 || yi == -1 || zi == -1)
89  {
90  return;
91  }
92 
93  const uint32_t xoff = cloud->fields[xi].offset;
94  const uint32_t yoff = cloud->fields[yi].offset;
95  const uint32_t zoff = cloud->fields[zi].offset;
96  const uint32_t point_step = cloud->point_step;
97  const size_t point_count = cloud->width * cloud->height;
98 
99  if( point_count * point_step != cloud->data.size() )
100  {
101  std::stringstream ss;
102  ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width
103  << ") times height (" << cloud->height << ") times point_step (" << point_step << "). Dropping message.";
104  setStatusStd( StatusProperty::Error, "Message", ss.str() );
105  return;
106  }
107 
108  filtered->data.resize(cloud->data.size());
109  uint32_t output_count;
110  if (point_count == 0)
111  {
112  output_count = 0;
113  } else {
114  uint8_t* output_ptr = &filtered->data.front();
115  const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
116  size_t points_to_copy = 0;
117  for (; ptr < ptr_end; ptr += point_step)
118  {
119  float x = *reinterpret_cast<const float*>(ptr + xoff);
120  float y = *reinterpret_cast<const float*>(ptr + yoff);
121  float z = *reinterpret_cast<const float*>(ptr + zoff);
122  if (validateFloats(x) && validateFloats(y) && validateFloats(z))
123  {
124  if (points_to_copy == 0)
125  {
126  // Only memorize where to start copying from
127  ptr_init = ptr;
128  points_to_copy = 1;
129  }
130  else
131  {
132  ++points_to_copy;
133  }
134  }
135  else
136  {
137  if (points_to_copy)
138  {
139  // Copy all the points that need to be copied
140  memcpy(output_ptr, ptr_init, point_step*points_to_copy);
141  output_ptr += point_step*points_to_copy;
142  points_to_copy = 0;
143  }
144  }
145  }
146  // Don't forget to flush what needs to be copied
147  if (points_to_copy)
148  {
149  memcpy(output_ptr, ptr_init, point_step*points_to_copy);
150  output_ptr += point_step*points_to_copy;
151  }
152  output_count = (output_ptr - &filtered->data.front()) / point_step;
153  }
154 
155  filtered->header = cloud->header;
156  filtered->fields = cloud->fields;
157  filtered->data.resize(output_count * point_step);
158  filtered->height = 1;
159  filtered->width = output_count;
160  filtered->is_bigendian = cloud->is_bigendian;
161  filtered->point_step = point_step;
162  filtered->row_step = output_count;
163 
164  point_cloud_common_->addMessage( filtered );
165 }
166 
167 
168 void PointCloud2Display::update( float wall_dt, float ros_dt )
169 {
170  point_cloud_common_->update( wall_dt, ros_dt );
171 }
172 
174 {
175  MFDClass::reset();
177 }
178 
179 } // namespace rviz
180 
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:157
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
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
virtual void reset()
Called to tell the display to clear its state.
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
tf::MessageFilter< sensor_msgs::PointCloud2 > * tf_filter_
TFSIMD_FORCE_INLINE const tfScalar & y() const
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.
virtual void setQueueSize(uint32_t new_queue_size)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
void setCallbackQueue(CallbackQueueInterface *queue)
virtual void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
Process a single message. Overridden from MessageFilterDisplay.
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Displays a point cloud of type sensor_msgs::PointCloud2.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
PointCloudCommon * point_cloud_common_
void update(float wall_dt, float ros_dt)
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51