grid_cells_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <boost/bind.hpp>
00031 
00032 #include <OgreSceneNode.h>
00033 #include <OgreSceneManager.h>
00034 #include <OgreManualObject.h>
00035 #include <OgreBillboardSet.h>
00036 
00037 #include <tf/transform_listener.h>
00038 
00039 #include "rviz/frame_manager.h"
00040 #include "rviz/ogre_helpers/arrow.h"
00041 #include "rviz/ogre_helpers/point_cloud.h"
00042 #include "rviz/properties/color_property.h"
00043 #include "rviz/properties/float_property.h"
00044 #include "rviz/properties/parse_color.h"
00045 #include "rviz/properties/ros_topic_property.h"
00046 #include "rviz/validate_floats.h"
00047 #include "rviz/display_context.h"
00048 
00049 #include "grid_cells_display.h"
00050 
00051 namespace rviz
00052 {
00053 
00054 GridCellsDisplay::GridCellsDisplay()
00055   : Display()
00056   , messages_received_(0)
00057   , last_frame_count_( uint64_t( -1 ))
00058 {
00059   color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
00060                                        "Color of the grid cells.", this );
00061 
00062   alpha_property_ = new FloatProperty( "Alpha", 1.0,
00063                                        "Amount of transparency to apply to the cells.",
00064                                        this, SLOT( updateAlpha() ));
00065   alpha_property_->setMin( 0 );
00066   alpha_property_->setMax( 1 );
00067 
00068   topic_property_ = new RosTopicProperty( "Topic", "",
00069                                           QString::fromStdString( ros::message_traits::datatype<nav_msgs::GridCells>() ),
00070                                           "nav_msgs::GridCells topic to subscribe to.",
00071                                           this, SLOT( updateTopic() ));
00072 }
00073 
00074 void GridCellsDisplay::onInitialize()
00075 {
00076   tf_filter_ = new tf::MessageFilter<nav_msgs::GridCells>( *context_->getTFClient(), fixed_frame_.toStdString(),
00077                                                            10, update_nh_ );
00078   static int count = 0;
00079   std::stringstream ss;
00080   ss << "PolyLine" << count++;
00081 
00082   cloud_ = new PointCloud();
00083   cloud_->setRenderMode( PointCloud::RM_TILES );
00084   cloud_->setCommonDirection( Ogre::Vector3::UNIT_Z );
00085   cloud_->setCommonUpVector( Ogre::Vector3::UNIT_Y );
00086   scene_node_->attachObject( cloud_ );
00087   updateAlpha();
00088 
00089   tf_filter_->connectInput( sub_ );
00090   tf_filter_->registerCallback( boost::bind( &GridCellsDisplay::incomingMessage, this, _1 ));
00091   context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00092 }
00093 
00094 GridCellsDisplay::~GridCellsDisplay()
00095 {
00096   if ( initialized() )
00097   {
00098     unsubscribe();
00099     clear();
00100     scene_node_->detachObject( cloud_ );
00101     delete cloud_;
00102     delete tf_filter_;
00103   }
00104 }
00105 
00106 void GridCellsDisplay::clear()
00107 {
00108   cloud_->clear();
00109 
00110   messages_received_ = 0;
00111   setStatus( StatusProperty::Warn, "Topic", "No messages received" );
00112 }
00113 
00114 void GridCellsDisplay::updateTopic()
00115 {
00116   unsubscribe();
00117   subscribe();
00118   context_->queueRender();
00119 }
00120 
00121 void GridCellsDisplay::updateAlpha()
00122 {
00123   cloud_->setAlpha( alpha_property_->getFloat() );
00124   context_->queueRender();
00125 }
00126 
00127 void GridCellsDisplay::subscribe()
00128 {
00129   if ( !isEnabled() )
00130   {
00131     return;
00132   }
00133 
00134   try
00135   {
00136     sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 );
00137     setStatus( StatusProperty::Ok, "Topic", "OK" );
00138   }
00139   catch( ros::Exception& e )
00140   {
00141     setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what() );
00142   }
00143 }
00144 
00145 void GridCellsDisplay::unsubscribe()
00146 {
00147   sub_.unsubscribe();
00148 }
00149 
00150 void GridCellsDisplay::onEnable()
00151 {
00152   subscribe();
00153 }
00154 
00155 void GridCellsDisplay::onDisable()
00156 {
00157   unsubscribe();
00158   clear();
00159 }
00160 
00161 void GridCellsDisplay::fixedFrameChanged()
00162 {
00163   clear();
00164 
00165   tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00166 }
00167 
00168 bool validateFloats(const nav_msgs::GridCells& msg)
00169 {
00170   bool valid = true;
00171   valid = valid && validateFloats( msg.cell_width );
00172   valid = valid && validateFloats( msg.cell_height );
00173   valid = valid && validateFloats( msg.cells );
00174   return valid;
00175 }
00176 
00177 void GridCellsDisplay::incomingMessage( const nav_msgs::GridCells::ConstPtr& msg )
00178 {
00179   if( !msg )
00180   {
00181     return;
00182   }
00183 
00184   ++messages_received_;
00185 
00186   if( context_->getFrameCount() == last_frame_count_ )
00187   {
00188     return;
00189   }
00190   last_frame_count_ = context_->getFrameCount();
00191 
00192   cloud_->clear();
00193 
00194   if( !validateFloats( *msg ))
00195   {
00196     setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00197     return;
00198   }
00199 
00200   setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00201 
00202   Ogre::Vector3 position;
00203   Ogre::Quaternion orientation;
00204   if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00205   {
00206     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00207                msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00208   }
00209 
00210   scene_node_->setPosition( position );
00211   scene_node_->setOrientation( orientation );
00212 
00213   if( msg->cell_width == 0 )
00214   {
00215     setStatus(StatusProperty::Error, "Topic", "Cell width is zero, cells will be invisible.");
00216   }
00217   else if( msg->cell_height == 0 )
00218   {
00219     setStatus(StatusProperty::Error, "Topic", "Cell height is zero, cells will be invisible.");
00220   }
00221 
00222   cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
00223 
00224   Ogre::ColourValue color_int = qtToOgre( color_property_->getColor() );
00225   uint32_t num_points = msg->cells.size();
00226 
00227   typedef std::vector< PointCloud::Point > V_Point;
00228   V_Point points;
00229   points.resize( num_points );
00230   for(uint32_t i = 0; i < num_points; i++)
00231   {
00232     PointCloud::Point& current_point = points[ i ];
00233     current_point.position.x = msg->cells[i].x;
00234     current_point.position.y = msg->cells[i].y;
00235     current_point.position.z = msg->cells[i].z;
00236     current_point.color = color_int;
00237   }
00238 
00239   cloud_->clear();
00240 
00241   if ( !points.empty() )
00242   {
00243     cloud_->addPoints( &points.front(), points.size() );
00244   }
00245 }
00246 
00247 void GridCellsDisplay::reset()
00248 {
00249   Display::reset();
00250   clear();
00251 }
00252 
00253 } // namespace rviz
00254 
00255 #include <pluginlib/class_list_macros.h>
00256 PLUGINLIB_EXPORT_CLASS( rviz::GridCellsDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27