odometry_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 "odometry_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 
00046 namespace rviz
00047 {
00048 
00049 OdometryDisplay::OdometryDisplay()
00050   : Display()
00051   , color_( 1.0f, 0.1f, 0.0f )
00052   , keep_(100)
00053   , length_( 1.0 )
00054   , position_tolerance_( 0.1 )
00055   , angle_tolerance_( 0.1 )
00056   , messages_received_(0)
00057 {
00058 }
00059 
00060 OdometryDisplay::~OdometryDisplay()
00061 {
00062   unsubscribe();
00063 
00064   clear();
00065 
00066   delete tf_filter_;
00067 }
00068 
00069 void OdometryDisplay::onInitialize()
00070 {
00071   tf_filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*vis_manager_->getTFClient(), "", 5, update_nh_);
00072   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00073 
00074   tf_filter_->connectInput(sub_);
00075   tf_filter_->registerCallback(boost::bind(&OdometryDisplay::incomingMessage, this, _1));
00076   vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00077 }
00078 
00079 void OdometryDisplay::clear()
00080 {
00081   D_Arrow::iterator it = arrows_.begin();
00082   D_Arrow::iterator end = arrows_.end();
00083   for ( ; it != end; ++it )
00084   {
00085     delete *it;
00086   }
00087   arrows_.clear();
00088 
00089   if (last_used_message_)
00090   {
00091     last_used_message_.reset();
00092   }
00093 
00094   tf_filter_->clear();
00095 
00096   messages_received_ = 0;
00097   setStatus(status_levels::Warn, "Topic", "No messages received");
00098 }
00099 
00100 void OdometryDisplay::setTopic( const std::string& topic )
00101 {
00102   unsubscribe();
00103   topic_ = topic;
00104   clear();
00105   subscribe();
00106 
00107   propertyChanged(topic_property_);
00108 
00109   causeRender();
00110 }
00111 
00112 void OdometryDisplay::setColor( const Color& color )
00113 {
00114   color_ = color;
00115 
00116   D_Arrow::iterator it = arrows_.begin();
00117   D_Arrow::iterator end = arrows_.end();
00118   for ( ; it != end; ++it )
00119   {
00120     Arrow* arrow = *it;
00121     arrow->setColor( color.r_, color.g_, color.b_, 1.0f );
00122   }
00123 
00124   propertyChanged(color_property_);
00125 
00126   causeRender();
00127 }
00128 
00129 void OdometryDisplay::setLength( float length )
00130 {
00131   length_ = length;
00132   D_Arrow::iterator it = arrows_.begin();
00133   D_Arrow::iterator end = arrows_.end();
00134   Ogre::Vector3 scale( length_, length_, length_ );
00135   for ( ; it != end; ++it )
00136   {
00137     Arrow* arrow = *it;
00138     arrow->setScale( scale );
00139   }
00140   propertyChanged( length_property_ );
00141   causeRender();
00142 }
00143 
00144 void OdometryDisplay::setKeep(uint32_t keep)
00145 {
00146   keep_ = keep;
00147 
00148   propertyChanged(keep_property_);
00149 }
00150 
00151 void OdometryDisplay::setPositionTolerance( float tol )
00152 {
00153   position_tolerance_ = tol;
00154 
00155   propertyChanged(position_tolerance_property_);
00156 }
00157 
00158 void OdometryDisplay::setAngleTolerance( float tol )
00159 {
00160   angle_tolerance_ = tol;
00161 
00162   propertyChanged(angle_tolerance_property_);
00163 }
00164 
00165 void OdometryDisplay::subscribe()
00166 {
00167   if ( !isEnabled() )
00168   {
00169     return;
00170   }
00171 
00172   try
00173   {
00174     sub_.subscribe(update_nh_, topic_, 5);
00175     setStatus(status_levels::Ok, "Topic", "OK");
00176   }
00177   catch (ros::Exception& e)
00178   {
00179     setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00180   }
00181 }
00182 
00183 void OdometryDisplay::unsubscribe()
00184 {
00185   sub_.unsubscribe();
00186 }
00187 
00188 void OdometryDisplay::onEnable()
00189 {
00190   scene_node_->setVisible( true );
00191   subscribe();
00192 }
00193 
00194 void OdometryDisplay::onDisable()
00195 {
00196   unsubscribe();
00197   clear();
00198   scene_node_->setVisible( false );
00199 }
00200 
00201 void OdometryDisplay::createProperties()
00202 {
00203   topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &OdometryDisplay::getTopic, this ),
00204                                                                                 boost::bind( &OdometryDisplay::setTopic, this, _1 ), parent_category_, this );
00205   setPropertyHelpText(topic_property_, "nav_msgs::Odometry topic to subscribe to.");
00206   ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00207   topic_prop->setMessageType(ros::message_traits::datatype<nav_msgs::Odometry>());
00208 
00209   color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &OdometryDisplay::getColor, this ),
00210                                                                           boost::bind( &OdometryDisplay::setColor, this, _1 ), parent_category_, this );
00211   setPropertyHelpText(color_property_, "Color of the arrows.");
00212 
00213   position_tolerance_property_ = property_manager_->createProperty<FloatProperty>( "Position Tolerance", property_prefix_, boost::bind( &OdometryDisplay::getPositionTolerance, this ),
00214                                                                                boost::bind( &OdometryDisplay::setPositionTolerance, this, _1 ), parent_category_, this );
00215   setPropertyHelpText(position_tolerance_property_, "Distance, in meters from the last arrow dropped, that will cause a new arrow to drop.");
00216   angle_tolerance_property_ = property_manager_->createProperty<FloatProperty>( "Angle Tolerance", property_prefix_, boost::bind( &OdometryDisplay::getAngleTolerance, this ),
00217                                                                                  boost::bind( &OdometryDisplay::setAngleTolerance, this, _1 ), parent_category_, this );
00218   setPropertyHelpText(angle_tolerance_property_, "Angular distance from the last arrow dropped, that will cause a new arrow to drop.");
00219 
00220   keep_property_ = property_manager_->createProperty<IntProperty>( "Keep", property_prefix_, boost::bind( &OdometryDisplay::getKeep, this ),
00221                                                                                boost::bind( &OdometryDisplay::setKeep, this, _1 ), parent_category_, this );
00222   setPropertyHelpText(keep_property_, "Number of arrows to keep before removing the oldest.");
00223 
00224   length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &OdometryDisplay::getLength, this ),
00225                                                                        boost::bind( &OdometryDisplay::setLength, this, _1 ), parent_category_, this );
00226   setPropertyHelpText(length_property_, "Length of each arrow.");
00227 }
00228 
00229 bool validateFloats(const nav_msgs::Odometry& msg)
00230 {
00231   bool valid = true;
00232   valid = valid && validateFloats(msg.pose.pose);
00233   valid = valid && validateFloats(msg.twist.twist);
00234   return valid;
00235 }
00236 
00237 void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& message )
00238 {
00239   ++messages_received_;
00240 
00241   if (!validateFloats(*message))
00242   {
00243     setStatus(status_levels::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00244     return;
00245   }
00246 
00247   {
00248     std::stringstream ss;
00249     ss << messages_received_ << " messages received";
00250     setStatus(status_levels::Ok, "Topic", ss.str());
00251   }
00252 
00253   if ( last_used_message_ )
00254   {
00255     Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
00256     Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
00257     Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
00258     Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
00259 
00260     if ((last_position - current_position).length() < position_tolerance_ && (last_orientation - current_orientation).normalise() < angle_tolerance_)
00261     {
00262       return;
00263     }
00264   }
00265 
00266   Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
00267 
00268   transformArrow( message, arrow );
00269 
00270   arrow->setColor( color_.r_, color_.g_, color_.b_, 1.0f );
00271   Ogre::Vector3 scale( length_, length_, length_ );
00272   arrow->setScale( scale );
00273   arrow->setUserData( Ogre::Any((void*)this) );
00274 
00275   arrows_.push_back( arrow );
00276   last_used_message_ = message;
00277 }
00278 
00279 void OdometryDisplay::transformArrow( const nav_msgs::Odometry::ConstPtr& message, Arrow* arrow )
00280 {
00281   Ogre::Vector3 position;
00282   Ogre::Quaternion orientation;
00283   if (!vis_manager_->getFrameManager()->transform(message->header, message->pose.pose, position, orientation))
00284   {
00285     ROS_ERROR( "Error transforming odometry '%s' from frame '%s' to frame '%s'", name_.c_str(), message->header.frame_id.c_str(), fixed_frame_.c_str() );
00286   }
00287 
00288   arrow->setPosition( position );
00289 
00290   // Arrow points in -Z direction, so rotate the orientation before display.
00291   // TODO: is it safe to change Arrow to point in +X direction?
00292   arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
00293 }
00294 
00295 void OdometryDisplay::fixedFrameChanged()
00296 {
00297   tf_filter_->setTargetFrame( fixed_frame_ );
00298   clear();
00299 }
00300 
00301 void OdometryDisplay::update(float wall_dt, float ros_dt)
00302 {
00303   if (keep_ > 0)
00304   {
00305     while (arrows_.size() > keep_)
00306     {
00307       delete arrows_.front();
00308       arrows_.pop_front();
00309     }
00310   }
00311 }
00312 
00313 void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
00314 {
00315   processMessage(message);
00316   causeRender();
00317 }
00318 
00319 void OdometryDisplay::reset()
00320 {
00321   Display::reset();
00322 
00323   clear();
00324 }
00325 
00326 } // namespace rviz


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