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 #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
00291
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 }