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 {
49 }
50 
52 {
54  delete point_cloud_common_;
55 }
56 
58 {
59  // Use the threaded queue for processing of incoming messages
61 
64 }
65 
66 void PointCloud2Display::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
67 {
68  // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase
69  // will get their points put off in lala land, but it means they still do get processed/rendered
70  // which can be a big performance hit
71  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
72  int32_t xi = findChannelIndex(cloud, "x");
73  int32_t yi = findChannelIndex(cloud, "y");
74  int32_t zi = findChannelIndex(cloud, "z");
75 
76  if (xi == -1 || yi == -1 || zi == -1)
77  {
78  return;
79  }
80 
81  const uint32_t xoff = cloud->fields[xi].offset;
82  const uint32_t yoff = cloud->fields[yi].offset;
83  const uint32_t zoff = cloud->fields[zi].offset;
84  const uint32_t point_step = cloud->point_step;
85  const size_t point_count = cloud->width * cloud->height;
86 
87  if (point_count * point_step != cloud->data.size())
88  {
89  std::stringstream ss;
90  ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width
91  << ") times height (" << cloud->height << ") times point_step (" << point_step
92  << "). Dropping message.";
93  setStatusStd(StatusProperty::Error, "Message", ss.str());
94  return;
95  }
96 
97  filtered->data.resize(cloud->data.size());
98  uint32_t output_count;
99  if (point_count == 0)
100  {
101  output_count = 0;
102  }
103  else
104  {
105  uint8_t* output_ptr = &filtered->data.front();
106  const uint8_t *ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
107  size_t points_to_copy = 0;
108  for (; ptr < ptr_end; ptr += point_step)
109  {
110  float x = *reinterpret_cast<const float*>(ptr + xoff);
111  float y = *reinterpret_cast<const float*>(ptr + yoff);
112  float z = *reinterpret_cast<const float*>(ptr + zoff);
113  if (validateFloats(x) && validateFloats(y) && validateFloats(z))
114  {
115  if (points_to_copy == 0)
116  {
117  // Only memorize where to start copying from
118  ptr_init = ptr;
119  points_to_copy = 1;
120  }
121  else
122  {
123  ++points_to_copy;
124  }
125  }
126  else
127  {
128  if (points_to_copy)
129  {
130  // Copy all the points that need to be copied
131  memcpy(output_ptr, ptr_init, point_step * points_to_copy);
132  output_ptr += point_step * points_to_copy;
133  points_to_copy = 0;
134  }
135  }
136  }
137  // Don't forget to flush what needs to be copied
138  if (points_to_copy)
139  {
140  memcpy(output_ptr, ptr_init, point_step * points_to_copy);
141  output_ptr += point_step * points_to_copy;
142  }
143  output_count = (output_ptr - &filtered->data.front()) / point_step;
144  }
145 
146  filtered->header = cloud->header;
147  filtered->fields = cloud->fields;
148  filtered->data.resize(output_count * point_step);
149  filtered->height = 1;
150  filtered->width = output_count;
151  filtered->is_bigendian = cloud->is_bigendian;
152  filtered->point_step = point_step;
153  filtered->row_step = output_count;
154 
155  point_cloud_common_->addMessage(filtered);
156 }
157 
158 
159 void PointCloud2Display::update(float wall_dt, float ros_dt)
160 {
161  point_cloud_common_->update(wall_dt, ros_dt);
162 }
163 
165 {
166  MFDClass::reset();
168 }
169 
170 } // namespace rviz
171 
rviz::findChannelIndex
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Definition: point_cloud_transformers.h:47
rviz::MessageFilterDisplay< sensor_msgs::PointCloud2 >::reset
void reset() override
Definition: message_filter_display.h:125
validate_floats.h
frame_manager.h
rviz::MessageFilterDisplay< sensor_msgs::PointCloud2 >::unsubscribe
virtual void unsubscribe()
Definition: message_filter_display.h:177
rviz::PointCloudCommon::initialize
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
Definition: point_cloud_common.cpp:366
rviz::StatusProperty::Error
@ Error
Definition: status_property.h:46
rviz::PointCloudCommon::update
void update(float wall_dt, float ros_dt)
Definition: point_cloud_common.cpp:520
time.h
point_cloud_common.h
int_property.h
rviz::validateFloats
bool validateFloats(const sensor_msgs::CameraInfo &msg)
Definition: camera_display.cpp:72
point_cloud.h
rviz::DisplayContext::getThreadedQueue
virtual ros::CallbackQueueInterface * getThreadedQueue()=0
Return a CallbackQueue using a different thread than the main GUI one.
rviz::PointCloudCommon::addMessage
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
Definition: point_cloud_common.cpp:939
rviz::PointCloud2Display::~PointCloud2Display
~PointCloud2Display() override
Definition: point_cloud2_display.cpp:51
rviz::PointCloud2Display
Displays a point cloud of type sensor_msgs::PointCloud2.
Definition: point_cloud2_display.h:52
rviz::Display
Definition: display.h:63
point_cloud2_display.h
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rviz
Definition: add_display_dialog.cpp:54
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
rviz::PointCloud2Display::reset
void reset() override
Called to tell the display to clear its state.
Definition: point_cloud2_display.cpp:164
rviz::PointCloud2Display::processMessage
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Process a single message. Overridden from MessageFilterDisplay.
Definition: point_cloud2_display.cpp:66
point_cloud_transformers.h
rviz::PointCloudCommon
Displays a point cloud of type sensor_msgs::PointCloud.
Definition: point_cloud_common.h:84
ros::NodeHandle::setCallbackQueue
void setCallbackQueue(CallbackQueueInterface *queue)
rviz::MessageFilterDisplay< sensor_msgs::PointCloud2 >::onInitialize
void onInitialize() override
Definition: message_filter_display.h:105
rviz::PointCloud2Display::onInitialize
void onInitialize() override
Do initialization. Overridden from MessageFilterDisplay.
Definition: point_cloud2_display.cpp:57
rviz::PointCloud2Display::PointCloud2Display
PointCloud2Display()
Definition: point_cloud2_display.cpp:47
rviz::Display::setStatusStd
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
rviz::Display::context_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz....
Definition: display.h:287
class_list_macros.hpp
rviz::PointCloud2Display::update
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
Definition: point_cloud2_display.cpp:159
rviz::PointCloudCommon::reset
void reset()
Definition: point_cloud_common.cpp:508
display_context.h
rviz::Display::update_nh_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
rviz::PointCloud2Display::point_cloud_common_
PointCloudCommon * point_cloud_common_
Definition: point_cloud2_display.h:70


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Sat Jun 1 2024 02:31:53