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 "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 }
00266