polygon_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 "polygon_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 #include "rviz/uniform_string_stream.h"
00039 
00040 #include <tf/transform_listener.h>
00041 
00042 #include <boost/bind.hpp>
00043 
00044 #include <OGRE/OgreSceneNode.h>
00045 #include <OGRE/OgreSceneManager.h>
00046 #include <OGRE/OgreManualObject.h>
00047 #include <OGRE/OgreBillboardSet.h>
00048 
00049 namespace rviz
00050 {
00051 
00052 PolygonDisplay::PolygonDisplay()
00053   : Display()
00054   , color_( 0.1f, 1.0f, 0.0f )
00055   , messages_received_(0)
00056 {
00057 }
00058 
00059 PolygonDisplay::~PolygonDisplay()
00060 {
00061   unsubscribe();
00062   clear();
00063 
00064   scene_manager_->destroyManualObject( manual_object_ );
00065   scene_manager_->destroySceneNode(scene_node_->getName());
00066   delete tf_filter_;
00067 }
00068 
00069 void PolygonDisplay::onInitialize()
00070 {
00071   tf_filter_ = new tf::MessageFilter<geometry_msgs::PolygonStamped>(*vis_manager_->getTFClient(), "", 10, update_nh_);
00072   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00073 
00074   static int count = 0;
00075   UniformStringStream ss;
00076   ss << "Polygon" << count++;
00077   manual_object_ = scene_manager_->createManualObject( ss.str() );
00078   manual_object_->setDynamic( true );
00079   scene_node_->attachObject( manual_object_ );
00080 
00081   setAlpha( 1.0f );
00082 
00083   tf_filter_->connectInput(sub_);
00084   tf_filter_->registerCallback(boost::bind(&PolygonDisplay::incomingMessage, this, _1));
00085   vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00086 }
00087 
00088 void PolygonDisplay::clear()
00089 {
00090   manual_object_->clear();
00091 
00092   messages_received_ = 0;
00093   setStatus(status_levels::Warn, "Topic", "No messages received");
00094 }
00095 
00096 void PolygonDisplay::setTopic( const std::string& topic )
00097 {
00098   unsubscribe();
00099 
00100   topic_ = topic;
00101 
00102   subscribe();
00103 
00104   propertyChanged(topic_property_);
00105 
00106   causeRender();
00107 }
00108 
00109 void PolygonDisplay::setColor( const Color& color )
00110 {
00111   color_ = color;
00112 
00113   propertyChanged(color_property_);
00114 
00115   processMessage(current_message_);
00116   causeRender();
00117 }
00118 
00119 void PolygonDisplay::setAlpha( float alpha )
00120 {
00121   alpha_ = alpha;
00122 
00123   propertyChanged(alpha_property_);
00124 
00125   processMessage(current_message_);
00126   causeRender();
00127 }
00128 
00129 void PolygonDisplay::subscribe()
00130 {
00131   if ( !isEnabled() )
00132   {
00133     return;
00134   }
00135 
00136   try
00137   {
00138     sub_.subscribe(update_nh_, topic_, 10);
00139     setStatus(status_levels::Ok, "Topic", "OK");
00140   }
00141   catch (ros::Exception& e)
00142   {
00143     setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00144   }
00145 }
00146 
00147 void PolygonDisplay::unsubscribe()
00148 {
00149   sub_.unsubscribe();
00150 }
00151 
00152 void PolygonDisplay::onEnable()
00153 {
00154   scene_node_->setVisible( true );
00155   subscribe();
00156 }
00157 
00158 void PolygonDisplay::onDisable()
00159 {
00160   unsubscribe();
00161   clear();
00162   scene_node_->setVisible( false );
00163 }
00164 
00165 void PolygonDisplay::fixedFrameChanged()
00166 {
00167   clear();
00168 
00169   tf_filter_->setTargetFrame( fixed_frame_ );
00170 }
00171 
00172 void PolygonDisplay::update(float wall_dt, float ros_dt)
00173 {
00174 }
00175 
00176 bool validateFloats(const geometry_msgs::PolygonStamped& msg)
00177 {
00178   return validateFloats(msg.polygon.points);
00179 }
00180 
00181 void PolygonDisplay::processMessage(const geometry_msgs::PolygonStamped::ConstPtr& msg)
00182 {
00183   if (!msg)
00184   {
00185     return;
00186   }
00187 
00188   ++messages_received_;
00189 
00190   if (!validateFloats(*msg))
00191   {
00192     setStatus(status_levels::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00193     return;
00194   }
00195 
00196   {
00197     std::stringstream ss;
00198     ss << messages_received_ << " messages received";
00199     setStatus(status_levels::Ok, "Topic", ss.str());
00200   }
00201 
00202   manual_object_->clear();
00203 
00204   Ogre::Vector3 position;
00205   Ogre::Quaternion orientation;
00206   if (!vis_manager_->getFrameManager()->getTransform(msg->header, position, orientation))
00207   {
00208     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00209   }
00210 
00211   scene_node_->setPosition( position );
00212   scene_node_->setOrientation( orientation );
00213 
00214   manual_object_->clear();
00215 
00216   Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, alpha_ );;
00217 
00218   uint32_t num_points = msg->polygon.points.size();
00219   if (num_points > 0)
00220   {
00221     manual_object_->estimateVertexCount( num_points );
00222     manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
00223     for( uint32_t i=0; i < num_points + 1; ++i)
00224     {
00225       Ogre::Vector3 pos(msg->polygon.points[i % num_points].x, msg->polygon.points[i % num_points].y, msg->polygon.points[i % num_points].z);
00226       manual_object_->position(pos);
00227       manual_object_->colour( color );
00228     }
00229 
00230     manual_object_->end();
00231   }
00232 }
00233 
00234 void PolygonDisplay::incomingMessage(const geometry_msgs::PolygonStamped::ConstPtr& msg)
00235 {
00236   processMessage(msg);
00237 }
00238 
00239 void PolygonDisplay::reset()
00240 {
00241   Display::reset();
00242   clear();
00243 }
00244 
00245 void PolygonDisplay::createProperties()
00246 {
00247   topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PolygonDisplay::getTopic, this ),
00248                                                                                 boost::bind( &PolygonDisplay::setTopic, this, _1 ), parent_category_, this );
00249   setPropertyHelpText(topic_property_, "geometry_msgs::Polygon topic to subscribe to.");
00250   ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00251   topic_prop->setMessageType(ros::message_traits::datatype<geometry_msgs::PolygonStamped>());
00252   color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PolygonDisplay::getColor, this ),
00253                                                                       boost::bind( &PolygonDisplay::setColor, this, _1 ), parent_category_, this );
00254   setPropertyHelpText(color_property_, "Color to draw the polygon.");
00255   alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &PolygonDisplay::getAlpha, this ),
00256                                                                        boost::bind( &PolygonDisplay::setAlpha, this, _1 ), parent_category_, this );
00257   setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the polygon.");
00258 }
00259 
00260 const char* PolygonDisplay::getDescription()
00261 {
00262   return "Displays data from a geometry_msgs::PolygonStamped message as lines.";
00263 }
00264 
00265 } // namespace rviz
00266 


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