range_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 "rviz/display_context.h"
34 #include "rviz/frame_manager.h"
40 
41 #include "range_display.h"
42 #include <limits>
43 
44 namespace rviz
45 {
47 {
48  color_property_ = new ColorProperty("Color", Qt::white, "Color to draw the range.", this,
49  SLOT(updateColorAndAlpha()));
50 
51  alpha_property_ = new FloatProperty("Alpha", 0.5, "Amount of transparency to apply to the range.",
52  this, SLOT(updateColorAndAlpha()));
53 
55  "Buffer Length", 1, "Number of prior measurements to display.", this, SLOT(updateBufferLength()));
57 
58  queue_size_property_ = new IntProperty("Queue Size", 100,
59  "Size of the tf message filter queue. It usually needs to be "
60  "set at least as high as the number of sonar frames.",
61  this, SLOT(updateQueueSize()));
62 }
63 
65 {
69 }
70 
72 {
73  for (size_t i = 0; i < cones_.size(); i++)
74  {
75  delete cones_[i];
76  }
77 }
78 
80 {
83 }
84 
86 {
88 }
89 
91 {
92  Ogre::ColourValue oc = color_property_->getOgreColor();
93  float alpha = alpha_property_->getFloat();
94  for (size_t i = 0; i < cones_.size(); i++)
95  {
96  cones_[i]->setColor(oc.r, oc.g, oc.b, alpha);
97  }
99 }
100 
102 {
103  int buffer_length = buffer_length_property_->getInt();
104  QColor color = color_property_->getColor();
105 
106  for (size_t i = 0; i < cones_.size(); i++)
107  {
108  delete cones_[i];
109  }
110  cones_.resize(buffer_length);
111  for (size_t i = 0; i < cones_.size(); i++)
112  {
114  cones_[i] = cone;
115 
116  Ogre::Vector3 position;
117  Ogre::Quaternion orientation;
118  geometry_msgs::Pose pose;
119  pose.orientation.w = 1;
120  Ogre::Vector3 scale(0, 0, 0);
121  cone->setScale(scale);
122  cone->setColor(color.redF(), color.greenF(), color.blueF(), 0);
123  }
124 }
125 
126 void RangeDisplay::processMessage(const sensor_msgs::Range::ConstPtr& msg)
127 {
129 
130  Ogre::Vector3 position;
131  Ogre::Quaternion orientation;
132  geometry_msgs::Pose pose;
133  float displayed_range = 0.0;
134  if (msg->min_range <= msg->range && msg->range <= msg->max_range)
135  {
136  displayed_range = msg->range;
137  }
138  else if (msg->min_range == msg->max_range)
139  { // Fixed distance ranger
140  if (msg->range < 0 && !std::isfinite(msg->range))
141  { // NaNs and +Inf return false here: both of those should have 0.0 as the range
142  displayed_range = msg->min_range; // -Inf, display the detectable range
143  }
144  }
145 
146  pose.position.x =
147  displayed_range / 2 -
148  .008824 * displayed_range; // .008824 fudge factor measured, must be inaccuracy of cone model.
149  pose.orientation.z = 0.707;
150  pose.orientation.w = 0.707;
151  if (!context_->getFrameManager()->transform(msg->header.frame_id, msg->header.stamp, pose, position,
152  orientation))
153  {
154  ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(),
155  qPrintable(fixed_frame_));
156  }
157 
158  cone->setPosition(position);
159  cone->setOrientation(orientation);
160 
161  double cone_width = 2.0 * displayed_range * tan(msg->field_of_view / 2.0);
162  Ogre::Vector3 scale(cone_width, displayed_range, cone_width);
163  cone->setScale(scale);
164 
165  QColor color = color_property_->getColor();
166  cone->setColor(color.redF(), color.greenF(), color.blueF(), alpha_property_->getFloat());
167 }
168 
169 } // namespace rviz
170 
ColorProperty * color_property_
Definition: range_display.h:77
tf2_ros::MessageFilter< sensor_msgs::Range > * tf_filter_
void processMessage(const sensor_msgs::Range::ConstPtr &msg) override
Overridden from MessageFilterDisplay.
~RangeDisplay() override
void setOrientation(const Ogre::Quaternion &orientation) override
Set the orientation of the object.
Definition: shape.cpp:155
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
virtual QColor getColor() const
void setScale(const Ogre::Vector3 &scale) override
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: shape.cpp:160
Ogre::ColourValue getOgreColor() const
void setMin(int min)
Property specialized to enforce floating point max/min.
void setColor(float r, float g, float b, float a) override
Set the color of the object. Values are in the range [0, 1].
Definition: shape.cpp:140
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
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
IntProperty * queue_size_property_
Definition: range_display.h:80
void reset() override
Overridden from Display.
FloatProperty * alpha_property_
Definition: range_display.h:78
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
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 setPosition(const Ogre::Vector3 &position) override
Set the position of this object.
Definition: shape.cpp:150
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
Displays a sensor_msgs::Range message as a cone.
Definition: range_display.h:52
virtual float getFloat() const
void onInitialize() override
Overridden from Display.
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::vector< Shape * > cones_
Handles actually drawing the cones.
Definition: range_display.h:75
IntProperty * buffer_length_property_
Definition: range_display.h:79
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Jan 9 2023 03:07:20