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