pose_array_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 "pose_array_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 
00047 namespace rviz
00048 {
00049 
00050 PoseArrayDisplay::PoseArrayDisplay()
00051   : Display()
00052   , color_( 1.0f, 0.1f, 0.0f )
00053   , length_( 0.3 )
00054   , messages_received_(0)
00055 {
00056 }
00057 
00058 PoseArrayDisplay::~PoseArrayDisplay()
00059 {
00060   unsubscribe();
00061   clear();
00062 
00063   scene_manager_->destroyManualObject( manual_object_ );
00064   delete tf_filter_;
00065 }
00066 
00067 void PoseArrayDisplay::onInitialize()
00068 {
00069   tf_filter_ = new tf::MessageFilter<geometry_msgs::PoseArray>(*vis_manager_->getTFClient(), "", 2, update_nh_);
00070   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00071 
00072   static int count = 0;
00073   std::stringstream ss;
00074   ss << "ParticleCloud2D" << count++;
00075   manual_object_ = scene_manager_->createManualObject( ss.str() );
00076   manual_object_->setDynamic( true );
00077   scene_node_->attachObject( manual_object_ );
00078 
00079   tf_filter_->connectInput(sub_);
00080   tf_filter_->registerCallback(boost::bind(&PoseArrayDisplay::incomingMessage, this, _1));
00081   vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00082 }
00083 
00084 void PoseArrayDisplay::clear()
00085 {
00086   manual_object_->clear();
00087 
00088   messages_received_ = 0;
00089   setStatus(status_levels::Warn, "Topic", "No messages received");
00090 }
00091 
00092 void PoseArrayDisplay::setTopic( const std::string& topic )
00093 {
00094   unsubscribe();
00095 
00096   topic_ = topic;
00097 
00098   subscribe();
00099 
00100   propertyChanged(topic_property_);
00101 
00102   causeRender();
00103 }
00104 
00105 void PoseArrayDisplay::setColor( const Color& color )
00106 {
00107   color_ = color;
00108 
00109   propertyChanged(color_property_);
00110 
00111   causeRender();
00112 }
00113 
00114 void PoseArrayDisplay::setLength( float length )
00115 {
00116   length_ = length;
00117   propertyChanged( length_property_ );
00118   causeRender();
00119 }
00120 
00121 void PoseArrayDisplay::subscribe()
00122 {
00123   if ( !isEnabled() )
00124   {
00125     return;
00126   }
00127 
00128   try
00129   {
00130     sub_.subscribe(update_nh_, topic_, 5);
00131     setStatus(status_levels::Ok, "Topic", "OK");
00132   }
00133   catch (ros::Exception& e)
00134   {
00135     setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00136   }
00137 }
00138 
00139 void PoseArrayDisplay::unsubscribe()
00140 {
00141   sub_.unsubscribe();
00142 }
00143 
00144 void PoseArrayDisplay::onEnable()
00145 {
00146   scene_node_->setVisible( true );
00147   subscribe();
00148 }
00149 
00150 void PoseArrayDisplay::onDisable()
00151 {
00152   unsubscribe();
00153   clear();
00154   scene_node_->setVisible( false );
00155 }
00156 
00157 void PoseArrayDisplay::createProperties()
00158 {
00159   topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PoseArrayDisplay::getTopic, this ),
00160                                                                                 boost::bind( &PoseArrayDisplay::setTopic, this, _1 ), parent_category_, this );
00161   setPropertyHelpText(topic_property_, "geometry_msgs::PoseArray topic to subscribe to.");
00162   ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00163   topic_prop->setMessageType(ros::message_traits::datatype<geometry_msgs::PoseArray>());
00164 
00165   color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PoseArrayDisplay::getColor, this ),
00166                                                                           boost::bind( &PoseArrayDisplay::setColor, this, _1 ), parent_category_, this );
00167   setPropertyHelpText(color_property_, "Color to draw the arrows.");
00168 
00169   length_property_ = property_manager_->createProperty<FloatProperty>( "Arrow Length", property_prefix_, boost::bind( &PoseArrayDisplay::getLength, this ),
00170                                                                       boost::bind( &PoseArrayDisplay::setLength, this, _1 ), parent_category_, this );
00171   setPropertyHelpText(length_property_, "Length of the arrows.");
00172 }
00173 
00174 void PoseArrayDisplay::fixedFrameChanged()
00175 {
00176   clear();
00177   tf_filter_->setTargetFrame( fixed_frame_ );
00178 }
00179 
00180 void PoseArrayDisplay::update(float wall_dt, float ros_dt)
00181 {
00182 }
00183 
00184 bool validateFloats(const geometry_msgs::PoseArray& msg)
00185 {
00186   return validateFloats(msg.poses);
00187 }
00188 
00189 void PoseArrayDisplay::processMessage(const geometry_msgs::PoseArray::ConstPtr& msg)
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   {
00200     std::stringstream ss;
00201     ss << messages_received_ << " messages received";
00202     setStatus(status_levels::Ok, "Topic", ss.str());
00203   }
00204 
00205   manual_object_->clear();
00206 
00207   Ogre::Vector3 position;
00208   Ogre::Quaternion orientation;
00209   if (!vis_manager_->getFrameManager()->getTransform(msg->header, position, orientation))
00210   {
00211     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00212   }
00213 
00214   scene_node_->setPosition( position );
00215   scene_node_->setOrientation( orientation );
00216 
00217   manual_object_->clear();
00218 
00219   Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, 1.0f );
00220   size_t num_poses = msg->poses.size();
00221   manual_object_->estimateVertexCount( num_poses * 6 );
00222   manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
00223   for( size_t i=0; i < num_poses; ++i)
00224   {
00225     Ogre::Vector3 pos(msg->poses[i].position.x, msg->poses[i].position.y, msg->poses[i].position.z);
00226     tf::Quaternion quat;
00227     tf::quaternionMsgToTF(msg->poses[i].orientation, quat);
00228     Ogre::Quaternion orient( quat.w(), quat.x(), quat.y(), quat.z() );
00229     // orient here is not normalized, so the scale of the quaternion
00230     // will affect the scale of the arrow.
00231 
00232     Ogre::Vector3 vertices[6];
00233     vertices[0] = pos; // back of arrow
00234     vertices[1] = pos + orient * Ogre::Vector3(length_, 0, 0); // tip of arrow
00235     vertices[2] = vertices[1];
00236     vertices[3] = pos + orient * Ogre::Vector3(0.75*length_, 0.2*length_, 0);
00237     vertices[4] = vertices[1];
00238     vertices[5] = pos + orient * Ogre::Vector3(0.75*length_, -0.2*length_, 0);
00239 
00240     for ( int i = 0; i < 6; ++i )
00241     {
00242       manual_object_->position( vertices[i] );
00243       manual_object_->colour( color );
00244     }
00245   }
00246   manual_object_->end();
00247 
00248   causeRender();
00249 }
00250 
00251 void PoseArrayDisplay::incomingMessage(const geometry_msgs::PoseArray::ConstPtr& msg)
00252 {
00253   processMessage(msg);
00254 }
00255 
00256 void PoseArrayDisplay::reset()
00257 {
00258   Display::reset();
00259   clear();
00260 }
00261 
00262 } // namespace rviz
00263 


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