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 
00031 #include <boost/bind.hpp>
00032 
00033 #include <tf/transform_listener.h>
00034 
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/ogre_helpers/arrow.h"
00037 #include "rviz/properties/color_property.h"
00038 #include "rviz/properties/float_property.h"
00039 #include "rviz/properties/int_property.h"
00040 #include "rviz/properties/ros_topic_property.h"
00041 #include "rviz/validate_floats.h"
00042 #include "rviz/display_context.h"
00043 
00044 #include "odometry_display.h"
00045 
00046 namespace rviz
00047 {
00048 
00049 OdometryDisplay::OdometryDisplay()
00050   : Display()
00051   , messages_received_(0)
00052 {
00053   topic_property_ = new RosTopicProperty( "Topic", "",
00054                                           QString::fromStdString( ros::message_traits::datatype<nav_msgs::Odometry>() ),
00055                                           "nav_msgs::Odometry topic to subscribe to.",
00056                                           this, SLOT( updateTopic() ));
00057 
00058   color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ),
00059                                        "Color of the arrows.",
00060                                        this, SLOT( updateColor() ));
00061 
00062   position_tolerance_property_ = new FloatProperty( "Position Tolerance", .1,
00063                                                     "Distance, in meters from the last arrow dropped, "
00064                                                     "that will cause a new arrow to drop.",
00065                                                     this );
00066   position_tolerance_property_->setMin( 0 );
00067                                                 
00068   angle_tolerance_property_ = new FloatProperty( "Angle Tolerance", .1,
00069                                                  "Angular distance from the last arrow dropped, "
00070                                                  "that will cause a new arrow to drop.",
00071                                                  this );
00072   angle_tolerance_property_->setMin( 0 );
00073 
00074   keep_property_ = new IntProperty( "Keep", 100,
00075                                     "Number of arrows to keep before removing the oldest.  0 means keep all of them.",
00076                                     this );
00077   keep_property_->setMin( 0 );
00078 
00079   length_property_ = new FloatProperty( "Length", 1.0,
00080                                         "Length of each arrow.",
00081                                         this, SLOT( updateLength() ));
00082 }
00083 
00084 OdometryDisplay::~OdometryDisplay()
00085 {
00086   if ( initialized() )
00087   {
00088     unsubscribe();
00089     clear();
00090     delete tf_filter_;
00091   }
00092 }
00093 
00094 void OdometryDisplay::onInitialize()
00095 {
00096   tf_filter_ = new tf::MessageFilter<nav_msgs::Odometry>( *context_->getTFClient(), fixed_frame_.toStdString(),
00097                                                           5, update_nh_ );
00098 
00099   tf_filter_->connectInput( sub_ );
00100   tf_filter_->registerCallback( boost::bind( &OdometryDisplay::incomingMessage, this, _1 ));
00101   context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00102 }
00103 
00104 void OdometryDisplay::clear()
00105 {
00106   D_Arrow::iterator it = arrows_.begin();
00107   D_Arrow::iterator end = arrows_.end();
00108   for ( ; it != end; ++it )
00109   {
00110     delete *it;
00111   }
00112   arrows_.clear();
00113 
00114   if( last_used_message_ )
00115   {
00116     last_used_message_.reset();
00117   }
00118 
00119   tf_filter_->clear();
00120 
00121   messages_received_ = 0;
00122   setStatus( StatusProperty::Warn, "Topic", "No messages received" );
00123 }
00124 
00125 void OdometryDisplay::updateTopic()
00126 {
00127   unsubscribe();
00128   clear();
00129   subscribe();
00130   context_->queueRender();
00131 }
00132 
00133 void OdometryDisplay::updateColor()
00134 {
00135   QColor color = color_property_->getColor();
00136   float red   = color.redF();
00137   float green = color.greenF();
00138   float blue  = color.blueF();
00139 
00140   D_Arrow::iterator it = arrows_.begin();
00141   D_Arrow::iterator end = arrows_.end();
00142   for( ; it != end; ++it )
00143   {
00144     Arrow* arrow = *it;
00145     arrow->setColor( red, green, blue, 1.0f );
00146   }
00147   context_->queueRender();
00148 }
00149 
00150 void OdometryDisplay::updateLength()
00151 {
00152   float length = length_property_->getFloat();
00153   D_Arrow::iterator it = arrows_.begin();
00154   D_Arrow::iterator end = arrows_.end();
00155   Ogre::Vector3 scale( length, length, length );
00156   for ( ; it != end; ++it )
00157   {
00158     Arrow* arrow = *it;
00159     arrow->setScale( scale );
00160   }
00161   context_->queueRender();
00162 }
00163 
00164 void OdometryDisplay::subscribe()
00165 {
00166   if ( !isEnabled() )
00167   {
00168     return;
00169   }
00170 
00171   try
00172   {
00173     sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 5 );
00174     setStatus( StatusProperty::Ok, "Topic", "OK" );
00175   }
00176   catch( ros::Exception& e )
00177   {
00178     setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00179   }
00180 }
00181 
00182 void OdometryDisplay::unsubscribe()
00183 {
00184   sub_.unsubscribe();
00185 }
00186 
00187 void OdometryDisplay::onEnable()
00188 {
00189   subscribe();
00190 }
00191 
00192 void OdometryDisplay::onDisable()
00193 {
00194   unsubscribe();
00195   clear();
00196 }
00197 
00198 bool validateFloats(const nav_msgs::Odometry& msg)
00199 {
00200   bool valid = true;
00201   valid = valid && validateFloats( msg.pose.pose );
00202   valid = valid && validateFloats( msg.twist.twist );
00203   return valid;
00204 }
00205 
00206 void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
00207 {
00208   ++messages_received_;
00209 
00210   if( !validateFloats( *message ))
00211   {
00212     setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00213     return;
00214   }
00215 
00216   setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00217 
00218   if( last_used_message_ )
00219   {
00220     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);
00221     Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
00222     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);
00223     Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
00224 
00225     if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
00226         (last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
00227     {
00228       return;
00229     }
00230   }
00231 
00232   Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
00233 
00234   transformArrow( message, arrow );
00235 
00236   QColor color = color_property_->getColor();
00237   arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
00238 
00239   float length = length_property_->getFloat();
00240   Ogre::Vector3 scale( length, length, length );
00241   arrow->setScale( scale );
00242 
00243   arrows_.push_back( arrow );
00244 
00245   last_used_message_ = message;
00246   context_->queueRender();
00247 }
00248 
00249 void OdometryDisplay::transformArrow( const nav_msgs::Odometry::ConstPtr& message, Arrow* arrow )
00250 {
00251   Ogre::Vector3 position;
00252   Ogre::Quaternion orientation;
00253   if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))
00254   {
00255     ROS_ERROR( "Error transforming odometry '%s' from frame '%s' to frame '%s'",
00256                qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00257   }
00258 
00259   arrow->setPosition( position );
00260 
00261   // Arrow points in -Z direction, so rotate the orientation before display.
00262   // TODO: is it safe to change Arrow to point in +X direction?
00263   arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
00264 }
00265 
00266 void OdometryDisplay::fixedFrameChanged()
00267 {
00268   tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00269   clear();
00270 }
00271 
00272 void OdometryDisplay::update( float wall_dt, float ros_dt )
00273 {
00274   size_t keep = keep_property_->getInt();
00275   if( keep > 0 )
00276   {
00277     while( arrows_.size() > keep )
00278     {
00279       delete arrows_.front();
00280       arrows_.pop_front();
00281     }
00282   }
00283 }
00284 
00285 void OdometryDisplay::reset()
00286 {
00287   Display::reset();
00288   clear();
00289 }
00290 
00291 void OdometryDisplay::setTopic( const QString &topic, const QString &datatype )
00292 {
00293   topic_property_->setString( topic );
00294 }
00295 
00296 } // namespace rviz
00297 
00298 #include <pluginlib/class_list_macros.h>
00299 PLUGINLIB_EXPORT_CLASS( rviz::OdometryDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27