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