illuminance_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 "illuminance_display.h"
44 
45 namespace rviz
46 {
47 IlluminanceDisplay::IlluminanceDisplay() : point_cloud_common_(new PointCloudCommon(this))
48 {
50  "Queue Size", 10,
51  "Advanced: set the size of the incoming Illuminance message queue. "
52  " Increasing this is useful if your incoming TF data is delayed significantly "
53  "from your Illuminance 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  // Set correct initial values
72  subProp("Channel Name")->setValue("illuminance");
73  subProp("Autocompute Intensity Bounds")->setValue(false);
74  subProp("Min Intensity")->setValue(0);
75  subProp("Max Intensity")->setValue(1000);
76 }
77 
79 {
81 }
82 
83 void IlluminanceDisplay::processMessage(const sensor_msgs::IlluminanceConstPtr& msg)
84 {
85  // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase
86  // will get their points put off in lala land, but it means they still do get processed/rendered
87  // which can be a big performance hit
88  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
89 
90  // Create fields
91  sensor_msgs::PointField x;
92  x.name = "x";
93  x.offset = 0;
94  x.datatype = sensor_msgs::PointField::FLOAT32;
95  x.count = 1;
96  sensor_msgs::PointField y;
97  y.name = "y";
98  y.offset = 4;
99  y.datatype = sensor_msgs::PointField::FLOAT32;
100  y.count = 1;
101  sensor_msgs::PointField z;
102  z.name = "z";
103  z.offset = 8;
104  z.datatype = sensor_msgs::PointField::FLOAT32;
105  z.count = 1;
106  sensor_msgs::PointField illuminance;
107  illuminance.name = "illuminance";
108  illuminance.offset = 12;
109  illuminance.datatype = sensor_msgs::PointField::FLOAT64;
110  illuminance.count = 1;
111 
112  // Create pointcloud from message
113  filtered->header = msg->header;
114  filtered->fields.push_back(x);
115  filtered->fields.push_back(y);
116  filtered->fields.push_back(z);
117  filtered->fields.push_back(illuminance);
118  filtered->data.resize(20);
119  const float zero_float = 0.0; // Illuminance is always on its tf frame
120  memcpy(&filtered->data[x.offset], &zero_float, 4);
121  memcpy(&filtered->data[y.offset], &zero_float, 4);
122  memcpy(&filtered->data[z.offset], &zero_float, 4);
123  memcpy(&filtered->data[illuminance.offset], &msg->illuminance, 8);
124  filtered->height = 1;
125  filtered->width = 1;
126  filtered->is_bigendian = false;
127  filtered->point_step = 20;
128  filtered->row_step = 1;
129 
130  // Give to point_cloud_common to draw
131  point_cloud_common_->addMessage(filtered);
132 }
133 
134 
135 void IlluminanceDisplay::update(float wall_dt, float ros_dt)
136 {
137  point_cloud_common_->update(wall_dt, ros_dt);
138 
139  // Hide unneeded properties
140  subProp("Position Transformer")->hide();
141  subProp("Color Transformer")->hide();
142  subProp("Channel Name")->hide();
143  subProp("Autocompute Intensity Bounds")->hide();
144 }
145 
147 {
148  MFDClass::reset();
150 }
151 
152 } // namespace rviz
153 
tf2_ros::MessageFilter< sensor_msgs::Illuminance > * tf_filter_
PointCloudCommon * point_cloud_common_
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
Displays an Illuminance message of type sensor_msgs::Illuminance.
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:134
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
void reset() override
Called to tell the display to clear its state.
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::IlluminanceConstPtr &msg) override
Process a single message. Overridden from MessageFilterDisplay.
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:179
void onInitialize() override
Do initialization. Overridden from MessageFilterDisplay.
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
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:401
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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 Sat May 27 2023 02:06:24