point_cloud_common.h
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 #ifndef RVIZ_POINT_CLOUD_COMMON_H
31 #define RVIZ_POINT_CLOUD_COMMON_H
32 
33 #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
34 #include <deque>
35 #include <queue>
36 #include <vector>
37 
38 #include <QObject>
39 #include <QList>
40 
41 #include <boost/shared_ptr.hpp>
42 #include <boost/thread/mutex.hpp>
43 #include <boost/thread/recursive_mutex.hpp>
44 
46 
48 
49 #include <sensor_msgs/PointCloud.h>
50 #include <sensor_msgs/PointCloud2.h>
51 
57 #endif
58 
59 namespace rviz
60 {
61 class BoolProperty;
62 class Display;
63 class DisplayContext;
64 class EnumProperty;
65 class FloatProperty;
66 struct IndexAndMessage;
71 
72 typedef std::vector<std::string> V_string;
73 
84 class PointCloudCommon : public QObject
85 {
86  Q_OBJECT
87 public:
88  struct CloudInfo
89  {
90  CloudInfo();
91  ~CloudInfo();
92 
93  // clear the point cloud, but keep selection handler around
94  void clear();
95 
97 
98  Ogre::SceneManager* manager_;
99 
100  sensor_msgs::PointCloud2ConstPtr message_;
101 
102  Ogre::SceneNode* scene_node_;
104  PointCloudSelectionHandlerPtr selection_handler_;
105 
106  std::vector<PointCloud::Point> transformed_points_;
107 
108  Ogre::Quaternion orientation_;
109  Ogre::Vector3 position_;
110  };
111 
113  typedef std::deque<CloudInfoPtr> D_CloudInfo;
114  typedef std::vector<CloudInfoPtr> V_CloudInfo;
115  typedef std::queue<CloudInfoPtr> Q_CloudInfo;
116  typedef std::list<CloudInfoPtr> L_CloudInfo;
117 
118  PointCloudCommon(Display* display);
119  ~PointCloudCommon() override;
120 
121  void initialize(DisplayContext* context, Ogre::SceneNode* scene_node);
122 
123  void fixedFrameChanged();
124  void reset();
125  void update(float wall_dt, float ros_dt);
126 
127  void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
128  void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
129 
131  {
132  return display_;
133  }
134 
136 
145 
146  void setAutoSize(bool auto_size);
147 
148 public Q_SLOTS:
149  void causeRetransform();
150 
151 private Q_SLOTS:
152  void updateSelectable();
153  void updateStyle();
154  void updateBillboardSize();
155  void updateAlpha();
156  void updateXyzTransformer();
157  void updateColorTransformer();
160 
161 private:
165  bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
166 
167  void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
168  void updateStatus();
169 
170  PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
171  PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
172  void updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud);
173  void retransform();
174  void onTransformerOptions(V_string& ops, uint32_t mask);
175 
176  void loadTransformers();
177 
178  float getSelectionBoxSize();
179  void setPropertiesHidden(const QList<Property*>& props, bool hide);
180  void fillTransformerOptions(EnumProperty* prop, uint32_t mask);
181 
182  D_CloudInfo cloud_infos_;
183 
184  Ogre::SceneNode* scene_node_;
185 
186  V_CloudInfo new_cloud_infos_;
187  boost::mutex new_clouds_mutex_;
188 
190 
192  {
193  PointCloudTransformerPtr transformer;
194  QList<Property*> xyz_props;
195  QList<Property*> color_props;
196 
197  std::string readable_name;
198  std::string lookup_name;
199  };
200  typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
201 
202  boost::recursive_mutex transformers_mutex_;
203  M_TransformerInfo transformers_;
207 
209 
212 
214 };
215 
217 {
218 public:
219  PointCloudSelectionHandler(float box_size,
220  PointCloudCommon::CloudInfo* cloud_info,
221  DisplayContext* context);
222  ~PointCloudSelectionHandler() override;
223 
224  void createProperties(const Picked& obj, Property* parent_property) override;
225  void destroyProperties(const Picked& obj, Property* parent_property) override;
226 
227  bool needsAdditionalRenderPass(uint32_t pass) override
228  {
229  return pass < 2;
230  }
231 
232  void preRenderPass(uint32_t pass) override;
233  void postRenderPass(uint32_t pass) override;
234 
235  void onSelect(const Picked& obj) override;
236  void onDeselect(const Picked& obj) override;
237 
238  void getAABBs(const Picked& obj, V_AABB& aabbs) override;
239 
240  void setBoxSize(float size)
241  {
242  box_size_ = size;
243  }
244 
246  {
247  return !boxes_.empty();
248  }
249 
250 private:
252  QHash<IndexAndMessage, Property*> property_hash_;
253  float box_size_;
254 };
255 
256 } // namespace rviz
257 
258 #endif // RVIZ_POINT_CLOUD_COMMON_H
void setXyzTransformerOptions(EnumProperty *prop)
FloatProperty * alpha_property_
pluginlib::ClassLoader< PointCloudTransformer > * transformer_class_loader_
void setPropertiesHidden(const QList< Property *> &props, bool hide)
void updateTransformers(const sensor_msgs::PointCloud2ConstPtr &cloud)
PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
std::vector< PointCloud::Point > transformed_points_
bool needsAdditionalRenderPass(uint32_t pass) override
std::map< std::string, TransformerInfo > M_TransformerInfo
FloatProperty * point_world_size_property_
boost::shared_ptr< CloudInfo > CloudInfoPtr
FloatProperty * decay_time_property_
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
EnumProperty * xyz_transformer_property_
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
friend class PointCloudSelectionHandler
Property specialized to enforce floating point max/min.
QHash< IndexAndMessage, Property * > property_hash_
boost::shared_ptr< PointCloudTransformer > PointCloudTransformerPtr
boost::recursive_mutex transformers_mutex_
std::list< CloudInfoPtr > L_CloudInfo
Displays a point cloud of type sensor_msgs::PointCloud.
PointCloudCommon(Display *display)
Pure-virtual base class for objects which give Display subclasses context in which to work...
std::vector< Ogre::AxisAlignedBox > V_AABB
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
EnumProperty * style_property_
void fillTransformerOptions(EnumProperty *prop, uint32_t mask)
EnumProperty * color_transformer_property_
BoolProperty * selectable_property_
PointCloudCommon::CloudInfo * cloud_info_
M_TransformerInfo transformers_
std::vector< CloudInfoPtr > V_CloudInfo
void setColorTransformerOptions(EnumProperty *prop)
FloatProperty * point_pixel_size_property_
std::vector< std::string > V_string
boost::shared_ptr< PointCloudSelectionHandler > PointCloudSelectionHandlerPtr
boost::shared_ptr< PointCloud > cloud_
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
void setAutoSize(bool auto_size)
Ogre::SceneNode * scene_node_
void onTransformerOptions(V_string &ops, uint32_t mask)
bool transformCloud(const CloudInfoPtr &cloud, bool fully_update_transformers)
Transforms the cloud into the correct frame, and sets up our renderable cloud.
sensor_msgs::PointCloud2ConstPtr message_
std::queue< CloudInfoPtr > Q_CloudInfo
Enum property.
Definition: enum_property.h:46
void update(float wall_dt, float ros_dt)
std::deque< CloudInfoPtr > D_CloudInfo
PointCloudSelectionHandlerPtr selection_handler_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat May 27 2023 02:06:25