fluid_pressure_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 "fluid_pressure_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 FluidPressure message queue. "
53  " Increasing this is useful if your incoming TF data is delayed significantly "
54  "from your FluidPressure 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  // Set correct initial values
73  subProp("Channel Name")->setValue("fluid_pressure");
74  subProp("Autocompute Intensity Bounds")->setValue(false);
75  subProp("Min Intensity")->setValue(98000); // Typical 'low' atmosphereic pressure in Pascal
76  subProp("Max Intensity")->setValue(105000); // Typica 'high' atmosphereic pressure in Pascal
77 }
78 
80 {
82 }
83 
84 void FluidPressureDisplay::processMessage( const sensor_msgs::FluidPressureConstPtr& msg )
85 {
86  // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase
87  // will get their points put off in lala land, but it means they still do get processed/rendered
88  // which can be a big performance hit
89  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
90 
91  // Create fields
92  sensor_msgs::PointField x;
93  x.name = "x";
94  x.offset = 0;
95  x.datatype = sensor_msgs::PointField::FLOAT32;
96  x.count = 1;
97  sensor_msgs::PointField y;
98  y.name = "y";
99  y.offset = 4;
100  y.datatype = sensor_msgs::PointField::FLOAT32;
101  y.count = 1;
102  sensor_msgs::PointField z;
103  z.name = "z";
104  z.offset = 8;
105  z.datatype = sensor_msgs::PointField::FLOAT32;
106  z.count = 1;
107  sensor_msgs::PointField fluid_pressure;
108  fluid_pressure.name = "fluid_pressure";
109  fluid_pressure.offset = 12;
110  fluid_pressure.datatype = sensor_msgs::PointField::FLOAT64;
111  fluid_pressure.count = 1;
112 
113  // Create pointcloud from message
114  filtered->header = msg->header;
115  filtered->fields.push_back(x);
116  filtered->fields.push_back(y);
117  filtered->fields.push_back(z);
118  filtered->fields.push_back(fluid_pressure);
119  filtered->data.resize(20);
120  const float zero_float = 0.0; // FluidPressure is always on its tf frame
121  memcpy(&filtered->data[x.offset], &zero_float, 4);
122  memcpy(&filtered->data[y.offset], &zero_float, 4);
123  memcpy(&filtered->data[z.offset], &zero_float, 4);
124  memcpy(&filtered->data[fluid_pressure.offset], &msg->fluid_pressure, 8);
125  filtered->height = 1;
126  filtered->width = 1;
127  filtered->is_bigendian = false;
128  filtered->point_step = 20;
129  filtered->row_step = 1;
130 
131  // Give to point_cloud_common to draw
132  point_cloud_common_->addMessage( filtered );
133 }
134 
135 
136 void FluidPressureDisplay::update( float wall_dt, float ros_dt )
137 {
138  point_cloud_common_->update( wall_dt, ros_dt );
139 
140  // Hide unneeded properties
141  subProp("Position Transformer")->hide();
142  subProp("Color Transformer")->hide();
143  subProp("Channel Name")->hide();
144  subProp("Autocompute Intensity Bounds")->hide();
145 }
146 
148 {
149  MFDClass::reset();
151 }
152 
153 } // namespace rviz
154 
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
Displays an FluidPressure message of type sensor_msgs::FluidPressure.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
Definition: property.cpp:130
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 processMessage(const sensor_msgs::FluidPressureConstPtr &msg)
Process a single message. Overridden from MessageFilterDisplay.
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
tf::MessageFilter< sensor_msgs::FluidPressure > * 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)
virtual Property * subProp(const QString &sub_name)
Return the first child Property with the given name, or the FailureProperty if no child has the name...
Definition: property.cpp:174
virtual void reset()
Called to tell the display to clear its state.
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:371
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
PointCloudCommon * point_cloud_common_
void update(float wall_dt, float ros_dt)


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