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 "grid_cells_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/frame_manager.h"
00035 #include "rviz/validate_floats.h"
00036
00037 #include "rviz/ogre_helpers/arrow.h"
00038
00039 #include <tf/transform_listener.h>
00040
00041 #include <boost/bind.hpp>
00042
00043 #include <OGRE/OgreSceneNode.h>
00044 #include <OGRE/OgreSceneManager.h>
00045 #include <OGRE/OgreManualObject.h>
00046 #include <OGRE/OgreBillboardSet.h>
00047
00048 #include <rviz/ogre_helpers/point_cloud.h>
00049
00050 namespace rviz
00051 {
00052
00053 GridCellsDisplay::GridCellsDisplay()
00054 : Display()
00055 , color_( 0.1f, 1.0f, 0.0f )
00056 , messages_received_(0)
00057 , last_frame_count_( uint64_t( -1 ))
00058 , hidden_(false)
00059 {
00060 }
00061
00062 void GridCellsDisplay::onInitialize()
00063 {
00064 tf_filter_ = new tf::MessageFilter<nav_msgs::GridCells>(*vis_manager_->getTFClient(), "", 10, update_nh_);
00065 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00066
00067 static int count = 0;
00068 std::stringstream ss;
00069 ss << "PolyLine" << count++;
00070
00071 cloud_ = new PointCloud();
00072 cloud_->setRenderMode( PointCloud::RM_BILLBOARDS_COMMON_FACING );
00073 cloud_->setCommonDirection( Ogre::Vector3::UNIT_Z );
00074 cloud_->setCommonUpVector( Ogre::Vector3::UNIT_Y );
00075 scene_node_->attachObject(cloud_);
00076 setAlpha( 1.0f );
00077
00078 tf_filter_->connectInput(sub_);
00079 tf_filter_->registerCallback(boost::bind(&GridCellsDisplay::incomingMessage, this, _1));
00080 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00081 }
00082
00083 GridCellsDisplay::~GridCellsDisplay()
00084 {
00085 unsubscribe();
00086 clear();
00087
00088 scene_manager_->destroySceneNode(scene_node_->getName());
00089 delete cloud_;
00090 delete tf_filter_;
00091 }
00092
00093 void GridCellsDisplay::clear()
00094 {
00095 cloud_->clear();
00096
00097 messages_received_ = 0;
00098 setStatus(status_levels::Warn, "Topic", "No messages received");
00099 }
00100
00101 void GridCellsDisplay::setTopic( const std::string& topic )
00102 {
00103 unsubscribe();
00104
00105 topic_ = topic;
00106
00107 subscribe();
00108
00109 propertyChanged(topic_property_);
00110
00111 causeRender();
00112 }
00113
00114 void GridCellsDisplay::setColor( const Color& color )
00115 {
00116 color_ = color;
00117
00118 propertyChanged(color_property_);
00119
00120 processMessage(current_message_);
00121 causeRender();
00122 }
00123
00124 void GridCellsDisplay::setAlpha( float alpha )
00125 {
00126 alpha_ = alpha;
00127
00128 cloud_->setAlpha( alpha );
00129
00130 propertyChanged(alpha_property_);
00131
00132 processMessage(current_message_);
00133 causeRender();
00134 }
00135
00136 void GridCellsDisplay::subscribe()
00137 {
00138 if ( !isEnabled() )
00139 {
00140 return;
00141 }
00142
00143 try
00144 {
00145 sub_.subscribe(update_nh_, topic_, 10);
00146 setStatus(status_levels::Ok, "Topic", "OK");
00147 }
00148 catch (ros::Exception& e)
00149 {
00150 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00151 }
00152 }
00153
00154 void GridCellsDisplay::unsubscribe()
00155 {
00156 sub_.unsubscribe();
00157 }
00158
00159
00160 void GridCellsDisplay::onEnable()
00161 {
00162 subscribe();
00163 scene_node_->setVisible( enabled_ && !hidden_ );
00164 }
00165
00166 void GridCellsDisplay::onDisable()
00167 {
00168 unsubscribe();
00169 scene_node_->setVisible( enabled_ && !hidden_ );
00170 clear();
00171 }
00172
00173 void GridCellsDisplay::hideVisible()
00174 {
00175 hidden_ = true;
00176 scene_node_->setVisible( enabled_ && !hidden_ );
00177 }
00178
00179 void GridCellsDisplay::restoreVisible()
00180 {
00181 hidden_ = false;
00182 scene_node_->setVisible( enabled_ && !hidden_ );
00183 }
00184
00185 void GridCellsDisplay::fixedFrameChanged()
00186 {
00187 clear();
00188
00189 tf_filter_->setTargetFrame( fixed_frame_ );
00190 }
00191
00192 void GridCellsDisplay::update(float wall_dt, float ros_dt)
00193 {
00194 }
00195
00196 bool validateFloats(const nav_msgs::GridCells& msg)
00197 {
00198 bool valid = true;
00199 valid = valid && validateFloats(msg.cell_width);
00200 valid = valid && validateFloats(msg.cell_height);
00201 valid = valid && validateFloats(msg.cells);
00202 return valid;
00203 }
00204
00205 void GridCellsDisplay::processMessage(const nav_msgs::GridCells::ConstPtr& msg)
00206 {
00207 if (!msg)
00208 {
00209 return;
00210 }
00211
00212 ++messages_received_;
00213
00214 if( vis_manager_->getFrameCount() == last_frame_count_ )
00215 {
00216 return;
00217 }
00218 last_frame_count_ = vis_manager_->getFrameCount();
00219
00220 cloud_->clear();
00221
00222 if (!validateFloats(*msg))
00223 {
00224 setStatus(status_levels::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00225 return;
00226 }
00227
00228 std::stringstream ss;
00229 ss << messages_received_ << " messages received";
00230 setStatus(status_levels::Ok, "Topic", ss.str());
00231
00232 Ogre::Vector3 position;
00233 Ogre::Quaternion orientation;
00234 if (!vis_manager_->getFrameManager()->getTransform(msg->header, position, orientation))
00235 {
00236 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00237 }
00238
00239 scene_node_->setPosition( position );
00240 scene_node_->setOrientation( orientation );
00241
00242 Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, alpha_ );
00243
00244 if( msg->cell_width == 0 )
00245 {
00246 setStatus(status_levels::Error, "Topic", "Cell width is zero, cells will be invisible.");
00247 }
00248 else if( msg->cell_height == 0 )
00249 {
00250 setStatus(status_levels::Error, "Topic", "Cell height is zero, cells will be invisible.");
00251 }
00252
00253 cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
00254
00255 uint32_t num_points = msg->cells.size();
00256
00257 typedef std::vector< PointCloud::Point > V_Point;
00258 V_Point points;
00259 points.resize( num_points );
00260 for(uint32_t i = 0; i < num_points; i++)
00261 {
00262 PointCloud::Point& current_point = points[ i ];
00263
00264 Ogre::Vector3 pos(msg->cells[i].x, msg->cells[i].y, msg->cells[i].z);
00265
00266 current_point.x = pos.x;
00267 current_point.y = pos.y;
00268 current_point.z = pos.z;
00269 current_point.setColor(color.r, color.g, color.b);
00270 }
00271
00272 cloud_->clear();
00273
00274 if ( !points.empty() )
00275 {
00276 cloud_->addPoints( &points.front(), points.size() );
00277 }
00278 }
00279
00280 void GridCellsDisplay::incomingMessage(const nav_msgs::GridCells::ConstPtr& msg)
00281 {
00282 processMessage(msg);
00283 }
00284
00285 void GridCellsDisplay::reset()
00286 {
00287 Display::reset();
00288 clear();
00289 }
00290
00291 void GridCellsDisplay::createProperties()
00292 {
00293 color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &GridCellsDisplay::getColor, this ),
00294 boost::bind( &GridCellsDisplay::setColor, this, _1 ), parent_category_, this );
00295 setPropertyHelpText(color_property_, "Color of the grid cells.");
00296 alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &GridCellsDisplay::getAlpha, this ),
00297 boost::bind( &GridCellsDisplay::setAlpha, this, _1 ), parent_category_, this );
00298 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the cells.");
00299
00300 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &GridCellsDisplay::getTopic, this ),
00301 boost::bind( &GridCellsDisplay::setTopic, this, _1 ), parent_category_, this );
00302 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00303 topic_prop->setMessageType(ros::message_traits::datatype<nav_msgs::GridCells>());
00304 setPropertyHelpText(topic_property_, "nav_msgs::GridCells topic to subscribe to.");
00305 }
00306
00307 const char* GridCellsDisplay::getDescription()
00308 {
00309 return "Displays data from a nav_msgs::GridCells message as either points or lines.";
00310 }
00311
00312 }
00313