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 <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
00262
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 }
00297
00298 #include <pluginlib/class_list_macros.h>
00299 PLUGINLIB_EXPORT_CLASS( rviz::OdometryDisplay, rviz::Display )