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 
45 # include <ros/spinner.h>
46 # include <ros/callback_queue.h>
47 
49 
51 
52 # include <sensor_msgs/PointCloud.h>
53 # include <sensor_msgs/PointCloud2.h>
54 
59 # include "rviz/selection/forwards.h"
60 #endif
61 
62 namespace rviz
63 {
64 class BoolProperty;
65 class Display;
66 class DisplayContext;
67 class EnumProperty;
68 class FloatProperty;
69 struct IndexAndMessage;
74 
75 typedef std::vector<std::string> V_string;
76 
85 class PointCloudCommon: public QObject
86 {
87 Q_OBJECT
88 public:
89  struct CloudInfo
90  {
91  CloudInfo();
92  ~CloudInfo();
93 
94  // clear the point cloud, but keep selection handler around
95  void clear();
96 
98 
99  Ogre::SceneManager *manager_;
100 
101  sensor_msgs::PointCloud2ConstPtr message_;
102 
103  Ogre::SceneNode *scene_node_;
105  PointCloudSelectionHandlerPtr selection_handler_;
106 
107  std::vector<PointCloud::Point> transformed_points_;
108 
109  Ogre::Quaternion orientation_;
110  Ogre::Vector3 position_;
111 };
112 
114  typedef std::deque<CloudInfoPtr> D_CloudInfo;
115  typedef std::vector<CloudInfoPtr> V_CloudInfo;
116  typedef std::queue<CloudInfoPtr> Q_CloudInfo;
117  typedef std::list<CloudInfoPtr> L_CloudInfo;
118 
119  PointCloudCommon( Display* display );
121 
122  void initialize( DisplayContext* context, Ogre::SceneNode* scene_node );
123 
124  void fixedFrameChanged();
125  void reset();
126  void update(float wall_dt, float ros_dt);
127 
128  void addMessage(const sensor_msgs::PointCloudConstPtr& cloud);
129  void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
130 
132 
133  Display* getDisplay() { return display_; }
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:
162 
166  bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
167 
168  void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud);
169  void updateStatus();
170 
171  PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
172  PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
173  void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
174  void retransform();
175  void onTransformerOptions(V_string& ops, uint32_t mask);
176 
177  void loadTransformers();
178 
179  float getSelectionBoxSize();
180  void setPropertiesHidden( const QList<Property*>& props, bool hide );
181  void fillTransformerOptions( EnumProperty* prop, uint32_t mask );
182 
185 
186  D_CloudInfo cloud_infos_;
187 
188  Ogre::SceneNode* scene_node_;
189 
190  V_CloudInfo new_cloud_infos_;
191  boost::mutex new_clouds_mutex_;
192 
194 
196  {
197  PointCloudTransformerPtr transformer;
198  QList<Property*> xyz_props;
199  QList<Property*> color_props;
200 
201  std::string readable_name;
202  std::string lookup_name;
203  };
204  typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
205 
206  boost::recursive_mutex transformers_mutex_;
207  M_TransformerInfo transformers_;
211 
213 
216 
218 };
219 
221 {
222 public:
223  PointCloudSelectionHandler( float box_size, PointCloudCommon::CloudInfo* cloud_info, DisplayContext* context );
224  virtual ~PointCloudSelectionHandler();
225 
226  virtual void createProperties( const Picked& obj, Property* parent_property );
227  virtual void destroyProperties( const Picked& obj, Property* parent_property );
228 
229  virtual bool needsAdditionalRenderPass(uint32_t pass)
230  {
231  if (pass < 2)
232  {
233  return true;
234  }
235 
236  return false;
237  }
238 
239  virtual void preRenderPass(uint32_t pass);
240  virtual void postRenderPass(uint32_t pass);
241 
242  virtual void onSelect(const Picked& obj);
243  virtual void onDeselect(const Picked& obj);
244 
245  virtual void getAABBs(const Picked& obj, V_AABB& aabbs);
246 
247  void setBoxSize( float size ) { box_size_=size; }
248 
249  bool hasSelections() { return !boxes_.empty(); }
250 
251 private:
253  QHash<IndexAndMessage, Property*> property_hash_;
254  float box_size_;
255 };
256 
257 } // namespace rviz
258 
259 #endif // RVIZ_POINT_CLOUD_COMMON_H
void setXyzTransformerOptions(EnumProperty *prop)
FloatProperty * alpha_property_
pluginlib::ClassLoader< PointCloudTransformer > * transformer_class_loader_
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_
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
ros::CallbackQueueInterface * getCallbackQueue()
EnumProperty * xyz_transformer_property_
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
ros::AsyncSpinner spinner_
void setPropertiesHidden(const QList< Property * > &props, bool hide)
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)
ros::CallbackQueue cbqueue_
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_
virtual bool needsAdditionalRenderPass(uint32_t pass)
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:47
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 Wed Aug 28 2019 04:01:51