grid_cells_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 } // namespace rviz
00313 


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32