grid_cells_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 <boost/bind.hpp>
31 
32 #include <OgreSceneNode.h>
33 #include <OgreSceneManager.h>
34 #include <OgreManualObject.h>
35 #include <OgreBillboardSet.h>
36 
37 #include <tf/transform_listener.h>
38 
39 #include "rviz/frame_manager.h"
46 #include "rviz/validate_floats.h"
47 #include "rviz/display_context.h"
48 
49 #include "grid_cells_display.h"
50 
51 namespace rviz
52 {
53 
55  : Display()
56  , messages_received_(0)
57  , last_frame_count_( uint64_t( -1 ))
58 {
59  color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
60  "Color of the grid cells.", this );
61 
62  alpha_property_ = new FloatProperty( "Alpha", 1.0,
63  "Amount of transparency to apply to the cells.",
64  this, SLOT( updateAlpha() ));
65  alpha_property_->setMin( 0 );
66  alpha_property_->setMax( 1 );
67 
68  topic_property_ = new RosTopicProperty( "Topic", "",
69  QString::fromStdString( ros::message_traits::datatype<nav_msgs::GridCells>() ),
70  "nav_msgs::GridCells topic to subscribe to.",
71  this, SLOT( updateTopic() ));
72 }
73 
75 {
77  10, update_nh_ );
78  static int count = 0;
79  std::stringstream ss;
80  ss << "PolyLine" << count++;
81 
82  cloud_ = new PointCloud();
84  cloud_->setCommonDirection( Ogre::Vector3::UNIT_Z );
85  cloud_->setCommonUpVector( Ogre::Vector3::UNIT_Y );
86  scene_node_->attachObject( cloud_ );
87  updateAlpha();
88 
92 }
93 
95 {
96  if ( initialized() )
97  {
98  unsubscribe();
99  clear();
100  scene_node_->detachObject( cloud_ );
101  delete cloud_;
102  delete tf_filter_;
103  }
104 }
105 
107 {
108  cloud_->clear();
109 
110  messages_received_ = 0;
111  setStatus( StatusProperty::Warn, "Topic", "No messages received" );
112 }
113 
115 {
116  unsubscribe();
117  subscribe();
119 }
120 
122 {
125 }
126 
128 {
129  if ( !isEnabled() )
130  {
131  return;
132  }
133 
134  try
135  {
137  setStatus( StatusProperty::Ok, "Topic", "OK" );
138  }
139  catch( ros::Exception& e )
140  {
141  setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what() );
142  }
143 }
144 
146 {
147  sub_.unsubscribe();
148 }
149 
151 {
152  subscribe();
153 }
154 
156 {
157  unsubscribe();
158  clear();
159 }
160 
162 {
163  clear();
164 
165  tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
166 }
167 
168 bool validateFloats(const nav_msgs::GridCells& msg)
169 {
170  bool valid = true;
171  valid = valid && validateFloats( msg.cell_width );
172  valid = valid && validateFloats( msg.cell_height );
173  valid = valid && validateFloats( msg.cells );
174  return valid;
175 }
176 
177 void GridCellsDisplay::incomingMessage( const nav_msgs::GridCells::ConstPtr& msg )
178 {
179  if( !msg )
180  {
181  return;
182  }
183 
185 
187  {
188  return;
189  }
191 
192  cloud_->clear();
193 
194  if( !validateFloats( *msg ))
195  {
196  setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
197  return;
198  }
199 
200  setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
201 
202  Ogre::Vector3 position;
203  Ogre::Quaternion orientation;
204  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
205  {
206  ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
207  msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
208  }
209 
210  scene_node_->setPosition( position );
211  scene_node_->setOrientation( orientation );
212 
213  if( msg->cell_width == 0 )
214  {
215  setStatus(StatusProperty::Error, "Topic", "Cell width is zero, cells will be invisible.");
216  }
217  else if( msg->cell_height == 0 )
218  {
219  setStatus(StatusProperty::Error, "Topic", "Cell height is zero, cells will be invisible.");
220  }
221 
222  cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
223 
224  Ogre::ColourValue color_int = qtToOgre( color_property_->getColor() );
225  uint32_t num_points = msg->cells.size();
226 
227  typedef std::vector< PointCloud::Point > V_Point;
228  V_Point points;
229  points.resize( num_points );
230  for(uint32_t i = 0; i < num_points; i++)
231  {
232  PointCloud::Point& current_point = points[ i ];
233  current_point.position.x = msg->cells[i].x;
234  current_point.position.y = msg->cells[i].y;
235  current_point.position.z = msg->cells[i].z;
236  current_point.color = color_int;
237  }
238 
239  cloud_->clear();
240 
241  if ( !points.empty() )
242  {
243  cloud_->addPoints( &points.front(), points.size() );
244  }
245 }
246 
248 {
249  Display::reset();
250  clear();
251 }
252 
253 } // namespace rviz
254 
virtual QColor getColor() const
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
void setMin(float min)
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
RosTopicProperty * topic_property_
void setMax(float max)
void connectInput(F &f)
message_filters::Subscriber< nav_msgs::GridCells > sub_
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
virtual uint64_t getFrameCount() const =0
Return the current value of the frame count.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:256
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:269
virtual float getFloat() const
FloatProperty * alpha_property_
void incomingMessage(const nav_msgs::GridCells::ConstPtr &msg)
Property specialized to enforce floating point max/min.
tf::MessageFilter< nav_msgs::GridCells > * tf_filter_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:264
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:281
Ogre::ColourValue qtToOgre(const QColor &c)
Definition: parse_color.cpp:87
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:281
void setCommonUpVector(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonUpVector.
Displays a nav_msgs::GridCells message.
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void reset()
Called to tell the display to clear its state.
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:300
void registerFilterForTransformStatusCheck(tf::MessageFilter< M > *filter, Display *display)
Connect a tf::MessageFilter&#39;s callbacks to success and failure handler functions in this FrameManager...
ColorProperty * color_property_
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:123
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
Ogre::Vector3 position
Definition: point_cloud.h:130
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void setAlpha(float alpha, bool per_point_alpha=false)
void clear()
Clear all the points.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
Ogre::ColourValue color
Definition: point_cloud.h:131
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
A visual representation of a set of points.
Definition: point_cloud.h:98
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
void setTargetFrame(const std::string &target_frame)
void setCommonDirection(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonDirection.
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:247
std::string getTopicStd() const
virtual void onInitialize()
Override this function to do subclass-specific initialization.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:186
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


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