vector3_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, 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/properties/tf_frame_property.h"
00042 #include "rviz/validate_floats.h"
00043 #include "rviz/display_context.h"
00044 
00045 #include "vector3_display.h"
00046 
00047 namespace hector_rviz_plugins
00048 {
00049 
00050 Vector3Display::Vector3Display()
00051   : Display()
00052   , messages_received_( 0 )
00053 {
00054   topic_property_ = new RosTopicProperty( "Topic", "",
00055                                           QString::fromStdString( ros::message_traits::datatype<geometry_msgs::Vector3Stamped>() ),
00056                                           "geometry_msgs::Vector3Stamped topic to subscribe to.",
00057                                           this, SLOT( updateTopic() ));
00058 
00059   color_property_ = new ColorProperty( "Color", QColor( 0, 25, 255 ),
00060                                        "Color of the arrows.",
00061                                        this, SLOT( updateColor() ));
00062 
00063   origin_frame_property_ = new TfFrameProperty( "Origin Frame", "base_link",
00064                                                 "Frame that defines the origin of the vector.",
00065                                                 this,
00066                                                 0, true,
00067                                                 SLOT( updateOriginFrame() ));
00068 
00069   position_tolerance_property_ = new FloatProperty( "Position Tolerance", .1,
00070                                                     "Distance, in meters from the last arrow dropped, "
00071                                                     "that will cause a new arrow to drop.",
00072                                                     this );
00073   position_tolerance_property_->setMin( 0 );
00074                                                 
00075   angle_tolerance_property_ = new FloatProperty( "Angle Tolerance", .1,
00076                                                  "Angular distance from the last arrow dropped, "
00077                                                  "that will cause a new arrow to drop.",
00078                                                  this );
00079   angle_tolerance_property_->setMin( 0 );
00080 
00081   scale_property_ = new FloatProperty( "Scale", 1.0,
00082                                         "Scale of each arrow.",
00083                                         this, SLOT( updateScale() ));
00084 
00085   keep_property_ = new IntProperty( "Keep", 100,
00086                                     "Number of arrows to keep before removing the oldest.  0 means keep all of them.",
00087                                     this );
00088   keep_property_->setMin( 0 );
00089 }
00090 
00091 Vector3Display::~Vector3Display()
00092 {
00093   unsubscribe();
00094   clear();
00095   delete tf_filter_;
00096 }
00097 
00098 void Vector3Display::onInitialize()
00099 {
00100   tf_filter_ = new tf::MessageFilter<geometry_msgs::Vector3Stamped>( *context_->getTFClient(), fixed_frame_.toStdString(),
00101                                                           5, update_nh_ );
00102 
00103   tf_filter_->connectInput( sub_ );
00104   tf_filter_->registerCallback( boost::bind( &Vector3Display::incomingMessage, this, _1 ));
00105   context_->getFrameManager()->registerFilterForTransformStatusCheck( tf_filter_, this );
00106 
00107   origin_frame_property_->setFrameManager(context_->getFrameManager());
00108 }
00109 
00110 // Clear the visuals by deleting their objects.
00111 void Vector3Display::clear()
00112 {
00113   D_Arrow::iterator it = arrows_.begin();
00114   D_Arrow::iterator end = arrows_.end();
00115   for ( ; it != end; ++it )
00116   {
00117     delete *it;
00118   }
00119   arrows_.clear();
00120 
00121   last_position_.reset();
00122   last_orientation_.reset();
00123 
00124   tf_filter_->clear();
00125 
00126   messages_received_ = 0;
00127   setStatus( StatusProperty::Warn, "Topic", "No messages received" );
00128 }
00129 
00130 void Vector3Display::updateTopic()
00131 {
00132   unsubscribe();
00133   clear();
00134   subscribe();
00135   context_->queueRender();
00136 }
00137 
00138 void Vector3Display::updateColor()
00139 {
00140   QColor color = color_property_->getColor();
00141   float red   = color.redF();
00142   float green = color.greenF();
00143   float blue  = color.blueF();
00144 
00145   D_Arrow::iterator it = arrows_.begin();
00146   D_Arrow::iterator end = arrows_.end();
00147   for( ; it != end; ++it )
00148   {
00149     Arrow* arrow = *it;
00150     arrow->setColor( red, green, blue, 1.0f );
00151   }
00152   context_->queueRender();
00153 }
00154 
00155 void Vector3Display::updateScale()
00156 {
00157 
00158 }
00159 
00160 void Vector3Display::updateOriginFrame()
00161 {
00162 
00163 }
00164 
00165 void Vector3Display::subscribe()
00166 {
00167   if ( !isEnabled() )
00168   {
00169     return;
00170   }
00171 
00172   try
00173   {
00174     sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 5 );
00175     setStatus( StatusProperty::Ok, "Topic", "OK" );
00176   }
00177   catch( ros::Exception& e )
00178   {
00179     setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00180   }
00181 }
00182 
00183 void Vector3Display::unsubscribe()
00184 {
00185   sub_.unsubscribe();
00186 }
00187 
00188 void Vector3Display::onEnable()
00189 {
00190   subscribe();
00191 }
00192 
00193 void Vector3Display::onDisable()
00194 {
00195   unsubscribe();
00196   clear();
00197 }
00198 
00199 // This is our callback to handle an incoming message.
00200 void Vector3Display::incomingMessage( const geometry_msgs::Vector3Stamped::ConstPtr& message )
00201 {
00202   ++messages_received_;
00203 
00204   if( !validateFloats( message->vector ))
00205   {
00206     setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00207     return;
00208   }
00209 
00210   setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
00211 
00212   // Here we call the rviz::FrameManager to get the transform from the
00213   // fixed frame to the frame in the header of this Vector3 message.  If
00214   // it fails, we can't do anything else so we return.
00215   Ogre::Quaternion orientation, fake_orientation;
00216   Ogre::Vector3 position, fake_position;
00217   if( !context_->getFrameManager()->getTransform( origin_frame_property_->getStdString(),
00218                                                   message->header.stamp,
00219                                                   position, fake_orientation ))
00220   {
00221     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00222                qPrintable( origin_frame_property_->getString() ), qPrintable( fixed_frame_ ) );
00223     return;
00224   }
00225 
00226   if( !context_->getFrameManager()->getTransform( message->header.frame_id,
00227                                                   message->header.stamp,
00228                                                   fake_position, orientation ))
00229   {
00230     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00231                message->header.frame_id.c_str(), qPrintable( fixed_frame_ ) );
00232     return;
00233   }
00234 
00235   Ogre::Vector3 vector( message->vector.x, message->vector.y, -message->vector.z );
00236   orientation = orientation * vector.getRotationTo(Ogre::Vector3::UNIT_Z);
00237 
00238   if( last_position_ && last_orientation_ )
00239   {
00240     if( (*last_position_ - position).length() < position_tolerance_property_->getFloat() &&
00241         (*last_orientation_ - orientation).normalise() < angle_tolerance_property_->getFloat() )
00242     {
00243       return;
00244     }
00245   }
00246 
00247   float length = vector.length();
00248   Arrow* arrow = new Arrow( scene_manager_, scene_node_, std::max(length - 0.2f, 0.0f), 0.05f, 0.2f, 0.2f );
00249 
00250   arrow->setPosition(position);
00251   arrow->setOrientation(orientation);
00252 
00253   QColor color = color_property_->getColor();
00254   arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
00255 
00256   Ogre::Vector3 scale(scale_property_->getFloat());
00257   arrow->setScale( scale );
00258 
00259   arrows_.push_back( arrow );
00260 
00261   if (!last_position_)    last_position_.reset(new Ogre::Vector3());
00262   if (!last_orientation_) last_orientation_.reset(new Ogre::Quaternion());
00263   *last_position_    = position;
00264   *last_orientation_ = orientation;
00265   context_->queueRender();
00266 }
00267 
00268 void Vector3Display::fixedFrameChanged()
00269 {
00270   tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00271   clear();
00272 }
00273 
00274 void Vector3Display::update( float wall_dt, float ros_dt )
00275 {
00276   size_t keep = keep_property_->getInt();
00277   if( keep > 0 )
00278   {
00279     while( arrows_.size() > keep )
00280     {
00281       delete arrows_.front();
00282       arrows_.pop_front();
00283     }
00284   }
00285 }
00286 
00287 void Vector3Display::reset()
00288 {
00289   Display::reset();
00290   clear();
00291 }
00292 
00293 } // namespace hector_rviz_plugins
00294 
00295 #include <pluginlib/class_list_macros.h>
00296 PLUGINLIB_EXPORT_CLASS( hector_rviz_plugins::Vector3Display, rviz::Display )


hector_rviz_plugins
Author(s): Johannes Meyer
autogenerated on Thu Aug 27 2015 13:22:38