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 <OGRE/OgreSceneNode.h>
00033 #include <OGRE/OgreSceneManager.h>
00034 #include <OGRE/OgreManualObject.h>
00035 #include <OGRE/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 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00079
00080 static int count = 0;
00081 std::stringstream ss;
00082 ss << "PolyLine" << count++;
00083
00084 cloud_ = new PointCloud();
00085 cloud_->setRenderMode( PointCloud::RM_BILLBOARDS_COMMON_FACING );
00086 cloud_->setCommonDirection( Ogre::Vector3::UNIT_Z );
00087 cloud_->setCommonUpVector( Ogre::Vector3::UNIT_Y );
00088 scene_node_->attachObject( cloud_ );
00089 updateAlpha();
00090
00091 tf_filter_->connectInput( sub_ );
00092 tf_filter_->registerCallback( boost::bind( &GridCellsDisplay::incomingMessage, this, _1 ));
00093 context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00094 }
00095
00096 GridCellsDisplay::~GridCellsDisplay()
00097 {
00098 unsubscribe();
00099 clear();
00100
00101 scene_manager_->destroySceneNode( scene_node_->getName() );
00102 delete cloud_;
00103 delete tf_filter_;
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 scene_node_->setVisible( true );
00153 subscribe();
00154 }
00155
00156 void GridCellsDisplay::onDisable()
00157 {
00158 unsubscribe();
00159 clear();
00160 scene_node_->setVisible( false );
00161 }
00162
00163 void GridCellsDisplay::fixedFrameChanged()
00164 {
00165 clear();
00166
00167 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00168 }
00169
00170 bool validateFloats(const nav_msgs::GridCells& msg)
00171 {
00172 bool valid = true;
00173 valid = valid && validateFloats( msg.cell_width );
00174 valid = valid && validateFloats( msg.cell_height );
00175 valid = valid && validateFloats( msg.cells );
00176 return valid;
00177 }
00178
00179 void GridCellsDisplay::incomingMessage( const nav_msgs::GridCells::ConstPtr& msg )
00180 {
00181 if( !msg )
00182 {
00183 return;
00184 }
00185
00186 ++messages_received_;
00187
00188 if( context_->getFrameCount() == last_frame_count_ )
00189 {
00190 return;
00191 }
00192 last_frame_count_ = context_->getFrameCount();
00193
00194 cloud_->clear();
00195
00196 if( !validateFloats( *msg ))
00197 {
00198 setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00199 return;
00200 }
00201
00202 setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00203
00204 Ogre::Vector3 position;
00205 Ogre::Quaternion orientation;
00206 if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00207 {
00208 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00209 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00210 }
00211
00212 scene_node_->setPosition( position );
00213 scene_node_->setOrientation( orientation );
00214
00215 if( msg->cell_width == 0 )
00216 {
00217 setStatus(StatusProperty::Error, "Topic", "Cell width is zero, cells will be invisible.");
00218 }
00219 else if( msg->cell_height == 0 )
00220 {
00221 setStatus(StatusProperty::Error, "Topic", "Cell height is zero, cells will be invisible.");
00222 }
00223
00224 cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
00225
00226 uint32_t color_int;
00227 Ogre::Root* root = Ogre::Root::getSingletonPtr();
00228 root->convertColourValue( qtToOgre( color_property_->getColor() ), &color_int );
00229
00230 uint32_t num_points = msg->cells.size();
00231
00232 typedef std::vector< PointCloud::Point > V_Point;
00233 V_Point points;
00234 points.resize( num_points );
00235 for(uint32_t i = 0; i < num_points; i++)
00236 {
00237 PointCloud::Point& current_point = points[ i ];
00238 current_point.x = msg->cells[i].x;
00239 current_point.y = msg->cells[i].y;
00240 current_point.z = msg->cells[i].z;
00241 current_point.color = color_int;
00242 }
00243
00244 cloud_->clear();
00245
00246 if ( !points.empty() )
00247 {
00248 cloud_->addPoints( &points.front(), points.size() );
00249 }
00250 }
00251
00252 void GridCellsDisplay::reset()
00253 {
00254 Display::reset();
00255 clear();
00256 }
00257
00258 }
00259
00260 #include <pluginlib/class_list_macros.h>
00261 PLUGINLIB_DECLARE_CLASS( rviz, GridCells, rviz::GridCellsDisplay, rviz::Display )