00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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 }
00254
00255 #include <pluginlib/class_list_macros.h>
00256 PLUGINLIB_EXPORT_CLASS( rviz::GridCellsDisplay, rviz::Display )