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