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