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 PointCloud2Display::PointCloud2Display() : point_cloud_common_(new PointCloudCommon(this))
48 {
50  "Queue Size", 10,
51  "Advanced: set the size of the incoming PointCloud2 message queue. "
52  " Increasing this is useful if your incoming TF data is delayed significantly "
53  "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.",
54  this, SLOT(updateQueueSize()));
55 }
56 
58 {
60  delete point_cloud_common_;
61 }
62 
64 {
65  // Use the threaded queue for processing of incoming messages
67 
70 }
71 
73 {
75 }
76 
77 void PointCloud2Display::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
78 {
79  // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase
80  // will get their points put off in lala land, but it means they still do get processed/rendered
81  // which can be a big performance hit
82  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
83  int32_t xi = findChannelIndex(cloud, "x");
84  int32_t yi = findChannelIndex(cloud, "y");
85  int32_t zi = findChannelIndex(cloud, "z");
86 
87  if (xi == -1 || yi == -1 || zi == -1)
88  {
89  return;
90  }
91 
92  const uint32_t xoff = cloud->fields[xi].offset;
93  const uint32_t yoff = cloud->fields[yi].offset;
94  const uint32_t zoff = cloud->fields[zi].offset;
95  const uint32_t point_step = cloud->point_step;
96  const size_t point_count = cloud->width * cloud->height;
97 
98  if (point_count * point_step != cloud->data.size())
99  {
100  std::stringstream ss;
101  ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width
102  << ") times height (" << cloud->height << ") times point_step (" << point_step
103  << "). 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  }
114  else
115  {
116  uint8_t* output_ptr = &filtered->data.front();
117  const uint8_t *ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
118  size_t points_to_copy = 0;
119  for (; ptr < ptr_end; ptr += point_step)
120  {
121  float x = *reinterpret_cast<const float*>(ptr + xoff);
122  float y = *reinterpret_cast<const float*>(ptr + yoff);
123  float z = *reinterpret_cast<const float*>(ptr + zoff);
124  if (validateFloats(x) && validateFloats(y) && validateFloats(z))
125  {
126  if (points_to_copy == 0)
127  {
128  // Only memorize where to start copying from
129  ptr_init = ptr;
130  points_to_copy = 1;
131  }
132  else
133  {
134  ++points_to_copy;
135  }
136  }
137  else
138  {
139  if (points_to_copy)
140  {
141  // Copy all the points that need to be copied
142  memcpy(output_ptr, ptr_init, point_step * points_to_copy);
143  output_ptr += point_step * points_to_copy;
144  points_to_copy = 0;
145  }
146  }
147  }
148  // Don't forget to flush what needs to be copied
149  if (points_to_copy)
150  {
151  memcpy(output_ptr, ptr_init, point_step * points_to_copy);
152  output_ptr += point_step * points_to_copy;
153  }
154  output_count = (output_ptr - &filtered->data.front()) / point_step;
155  }
156 
157  filtered->header = cloud->header;
158  filtered->fields = cloud->fields;
159  filtered->data.resize(output_count * point_step);
160  filtered->height = 1;
161  filtered->width = output_count;
162  filtered->is_bigendian = cloud->is_bigendian;
163  filtered->point_step = point_step;
164  filtered->row_step = output_count;
165 
166  point_cloud_common_->addMessage(filtered);
167 }
168 
169 
170 void PointCloud2Display::update(float wall_dt, float ros_dt)
171 {
172  point_cloud_common_->update(wall_dt, ros_dt);
173 }
174 
176 {
177  MFDClass::reset();
179 }
180 
181 } // namespace rviz
182 
tf2_ros::MessageFilter< sensor_msgs::PointCloud2 > * tf_filter_
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:163
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
void onInitialize() override
Do initialization. Overridden from MessageFilterDisplay.
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
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.
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Process a single message. Overridden from MessageFilterDisplay.
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
virtual void setQueueSize(uint32_t new_queue_size)
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
void reset() override
Called to tell the display to clear its state.
Displays a point cloud of type sensor_msgs::PointCloud2.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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)


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