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,
49  "Color to draw the range.",
50  this, SLOT( updateColorAndAlpha() ));
51 
52  alpha_property_ = new FloatProperty( "Alpha", 0.5,
53  "Amount of transparency to apply to the range.",
54  this, SLOT( updateColorAndAlpha() ));
55 
56  buffer_length_property_ = new IntProperty( "Buffer Length", 1,
57  "Number of prior measurements to display.",
58  this, SLOT( updateBufferLength() ));
60 
61  queue_size_property_ = new IntProperty( "Queue Size", 100,
62  "Size of the tf message filter queue. It usually needs to be set at least as high as the number of sonar frames.",
63  this, SLOT( updateQueueSize() ));
64 }
65 
67 {
71 }
72 
74 {
75  for( size_t i = 0; i < cones_.size(); i++ )
76  {
77  delete cones_[ i ];
78  }
79 }
80 
82 {
85 }
86 
88 {
90 }
91 
93 {
94  Ogre::ColourValue oc = color_property_->getOgreColor();
95  float alpha = alpha_property_->getFloat();
96  for( size_t i = 0; i < cones_.size(); i++ )
97  {
98  cones_[i]->setColor( oc.r, oc.g, oc.b, alpha );
99  }
101 }
102 
104 {
105  int buffer_length = buffer_length_property_->getInt();
106  QColor color = color_property_->getColor();
107 
108  for( size_t i = 0; i < cones_.size(); i++ )
109  {
110  delete cones_[i];
111  }
112  cones_.resize( buffer_length );
113  for( size_t i = 0; i < cones_.size(); i++ )
114  {
116  cones_[ i ] = cone;
117 
118  Ogre::Vector3 position;
119  Ogre::Quaternion orientation;
120  geometry_msgs::Pose pose;
121  pose.orientation.w = 1;
122  Ogre::Vector3 scale( 0, 0, 0 );
123  cone->setScale( scale );
124  cone->setColor( color.redF(), color.greenF(), color.blueF(), 0 );
125  }
126 }
127 
128 void RangeDisplay::processMessage( const sensor_msgs::Range::ConstPtr& msg )
129 {
131 
132  Ogre::Vector3 position;
133  Ogre::Quaternion orientation;
134  geometry_msgs::Pose pose;
135  float displayed_range = 0.0;
136  if(msg->min_range <= msg->range && msg->range <= msg->max_range){
137  displayed_range = msg->range;
138  } else if(msg->min_range == msg->max_range){ // Fixed distance ranger
139  if(msg->range < 0 && !std::isfinite(msg->range)){ // NaNs and +Inf return false here: both of those should have 0.0 as the range
140  displayed_range = msg->min_range; // -Inf, display the detectable range
141  }
142  }
143 
144  pose.position.x = displayed_range/2 - .008824 * displayed_range; // .008824 fudge factor measured, must be inaccuracy of cone model.
145  pose.orientation.z = 0.707;
146  pose.orientation.w = 0.707;
147  if( !context_->getFrameManager()->transform( msg->header.frame_id, msg->header.stamp, pose, position, orientation ))
148  {
149  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
150  msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
151  }
152 
153  cone->setPosition( position );
154  cone->setOrientation( orientation );
155 
156  double cone_width = 2.0 * displayed_range * tan( msg->field_of_view / 2.0 );
157  Ogre::Vector3 scale( cone_width, displayed_range, cone_width );
158  cone->setScale( scale );
159 
160  QColor color = color_property_->getColor();
161  cone->setColor( color.redF(), color.greenF(), color.blueF(), alpha_property_->getFloat() );
162 }
163 
164 } // namespace rviz
165 
std::vector< Shape * > cones_
Handles actually drawing the cones.
Definition: range_display.h:76
virtual QColor getColor() const
ColorProperty * color_property_
Definition: range_display.h:78
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: shape.cpp:157
Ogre::ColourValue getOgreColor() const
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:73
virtual float getFloat() const
void setMin(int min)
tf::MessageFilter< sensor_msgs::Range > * tf_filter_
Property specialized to enforce floating point max/min.
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
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
Definition: shape.cpp:142
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
virtual void setQueueSize(uint32_t new_queue_size)
virtual void onInitialize()
Overridden from Display.
IntProperty * queue_size_property_
Definition: range_display.h:81
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: shape.cpp:152
FloatProperty * alpha_property_
Definition: range_display.h:79
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 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:53
virtual void processMessage(const sensor_msgs::Range::ConstPtr &msg)
Overridden from MessageFilterDisplay.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: shape.cpp:162
IntProperty * buffer_length_property_
Definition: range_display.h:80
virtual void reset()
Overridden from Display.
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42