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 "pose_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/selection/selection_manager.h"
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/validate_floats.h"
00037
00038 #include "rviz/ogre_helpers/arrow.h"
00039 #include "rviz/ogre_helpers/axes.h"
00040
00041 #include <tf/transform_listener.h>
00042
00043 #include <boost/bind.hpp>
00044
00045 #include <OGRE/OgreSceneNode.h>
00046 #include <OGRE/OgreSceneManager.h>
00047
00048 namespace rviz
00049 {
00050
00051 class PoseDisplaySelectionHandler : public SelectionHandler
00052 {
00053 public:
00054 PoseDisplaySelectionHandler(const std::string& name)
00055 : name_(name)
00056 {}
00057
00058 void createProperties(const Picked& obj, PropertyManager* property_manager)
00059 {
00060 std::stringstream prefix;
00061 prefix << "Pose " << name_;
00062 CategoryPropertyWPtr cat = property_manager->createCategory(prefix.str(), prefix.str());
00063 properties_.push_back(property_manager->createProperty<StringProperty>("Frame", prefix.str(), boost::bind(&PoseDisplaySelectionHandler::getFrame, this), StringProperty::Setter(), cat));
00064 properties_.push_back(property_manager->createProperty<Vector3Property>("Position", prefix.str(), boost::bind(&PoseDisplaySelectionHandler::getPosition, this), Vector3Property::Setter(), cat));
00065 properties_.push_back(property_manager->createProperty<QuaternionProperty>("Orientation", prefix.str(), boost::bind(&PoseDisplaySelectionHandler::getOrientation, this), QuaternionProperty::Setter(), cat));
00066 }
00067
00068 void setMessage(const geometry_msgs::PoseStampedConstPtr& message)
00069 {
00070 message_ = message;
00071 }
00072
00073 std::string getFrame()
00074 {
00075 return message_->header.frame_id;
00076 }
00077
00078 Ogre::Vector3 getPosition()
00079 {
00080 return Ogre::Vector3(message_->pose.position.x, message_->pose.position.y, message_->pose.position.z);
00081 }
00082
00083 Ogre::Quaternion getOrientation()
00084 {
00085 return Ogre::Quaternion(message_->pose.orientation.x, message_->pose.orientation.y, message_->pose.orientation.z, message_->pose.orientation.w);
00086 }
00087
00088 private:
00089 std::string name_;
00090 geometry_msgs::PoseStampedConstPtr message_;
00091 };
00092
00093 PoseDisplay::PoseDisplay()
00094 : Display()
00095 , color_( 1.0f, 0.1f, 0.0f )
00096 , head_radius_(0.2)
00097 , head_length_(0.3)
00098 , shaft_radius_(0.1)
00099 , shaft_length_(1.0)
00100 , axes_length_(1.0)
00101 , axes_radius_(0.1)
00102 , messages_received_(0)
00103 {
00104 }
00105
00106 void PoseDisplay::onInitialize()
00107 {
00108 tf_filter_ = new tf::MessageFilter<geometry_msgs::PoseStamped>(*vis_manager_->getTFClient(), "", 5, update_nh_);
00109 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00110
00111 tf_filter_->connectInput(sub_);
00112 tf_filter_->registerCallback(boost::bind(&PoseDisplay::incomingMessage, this, _1));
00113 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00114
00115 arrow_ = new rviz::Arrow(scene_manager_, scene_node_, shaft_length_, shaft_radius_, head_length_, head_radius_);
00116
00117
00118 arrow_->setOrientation( Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));
00119
00120 axes_ = new rviz::Axes(scene_manager_, scene_node_, axes_length_, axes_radius_);
00121
00122 setVisibility();
00123
00124 Ogre::Quaternion quat(Ogre::Quaternion::IDENTITY);
00125 axes_->setOrientation(quat);
00126
00127 SelectionManager* sel_manager = vis_manager_->getSelectionManager();
00128 coll_handler_.reset(new PoseDisplaySelectionHandler(name_));
00129 coll_ = sel_manager->createCollisionForObject(arrow_, coll_handler_);
00130 sel_manager->createCollisionForObject(axes_, coll_handler_, coll_);
00131
00132 setShape(Arrow);
00133 setAlpha(1.0);
00134 }
00135
00136 PoseDisplay::~PoseDisplay()
00137 {
00138 unsubscribe();
00139
00140 clear();
00141
00142 SelectionManager* sel_manager = vis_manager_->getSelectionManager();
00143 sel_manager->removeObject(coll_);
00144
00145 delete arrow_;
00146 delete axes_;
00147 delete tf_filter_;
00148 }
00149
00150 void PoseDisplay::clear()
00151 {
00152 tf_filter_->clear();
00153 latest_message_.reset();
00154 setVisibility();
00155
00156 messages_received_ = 0;
00157 setStatus(status_levels::Warn, "Topic", "No messages received");
00158 }
00159
00160 void PoseDisplay::setTopic( const std::string& topic )
00161 {
00162 unsubscribe();
00163 topic_ = topic;
00164 subscribe();
00165
00166 propertyChanged(topic_property_);
00167
00168 causeRender();
00169 }
00170
00171 void PoseDisplay::setColor( const Color& color )
00172 {
00173 color_ = color;
00174
00175 arrow_->setColor(color.r_, color.g_, color.b_, alpha_);
00176 axes_->setColor(color.r_, color.g_, color.b_, alpha_);
00177
00178 propertyChanged(color_property_);
00179
00180 causeRender();
00181 }
00182
00183 void PoseDisplay::setAlpha( float a )
00184 {
00185 alpha_ = a;
00186
00187 arrow_->setColor(color_.r_, color_.g_, color_.b_, alpha_);
00188 axes_->setColor(color_.r_, color_.g_, color_.b_, alpha_);
00189
00190 propertyChanged(alpha_property_);
00191
00192 causeRender();
00193 }
00194
00195 void PoseDisplay::setHeadRadius(float r)
00196 {
00197 head_radius_ = r;
00198 arrow_->set(shaft_length_, shaft_radius_, head_length_, head_radius_);
00199 propertyChanged(head_radius_property_);
00200 }
00201
00202 void PoseDisplay::setHeadLength(float l)
00203 {
00204 head_length_ = l;
00205 arrow_->set(shaft_length_, shaft_radius_, head_length_, head_radius_);
00206 propertyChanged(head_length_property_);
00207 }
00208
00209 void PoseDisplay::setShaftRadius(float r)
00210 {
00211 shaft_radius_ = r;
00212 arrow_->set(shaft_length_, shaft_radius_, head_length_, head_radius_);
00213 propertyChanged(shaft_radius_property_);
00214 }
00215
00216 void PoseDisplay::setShaftLength(float l)
00217 {
00218 shaft_length_ = l;
00219 arrow_->set(shaft_length_, shaft_radius_, head_length_, head_radius_);
00220 propertyChanged(shaft_length_property_);
00221 }
00222
00223 void PoseDisplay::setAxesRadius(float r)
00224 {
00225 axes_radius_ = r;
00226 axes_->set(axes_length_, axes_radius_);
00227 propertyChanged(axes_radius_property_);
00228 }
00229
00230 void PoseDisplay::setAxesLength(float l)
00231 {
00232 axes_length_ = l;
00233 axes_->set(axes_length_, axes_radius_);
00234 propertyChanged(axes_length_property_);
00235 }
00236
00237 void PoseDisplay::setShape(int shape)
00238 {
00239 current_shape_ = (Shape)shape;
00240
00241 setVisibility();
00242
00243 propertyChanged(shape_property_);
00244
00245 createShapeProperties();
00246
00247 causeRender();
00248 }
00249
00250 void PoseDisplay::setVisibility()
00251 {
00252 arrow_->getSceneNode()->setVisible(false);
00253 axes_->getSceneNode()->setVisible(false);
00254
00255 if (!latest_message_)
00256 {
00257 return;
00258 }
00259
00260 switch (current_shape_)
00261 {
00262 case Arrow:
00263 arrow_->getSceneNode()->setVisible(true);
00264 break;
00265 case Axes:
00266 axes_->getSceneNode()->setVisible(true);
00267 break;
00268 }
00269 }
00270
00271 void PoseDisplay::subscribe()
00272 {
00273 if ( !isEnabled() )
00274 {
00275 return;
00276 }
00277
00278 try
00279 {
00280 sub_.subscribe(update_nh_, topic_, 5);
00281 setStatus(status_levels::Ok, "Topic", "OK");
00282 }
00283 catch (ros::Exception& e)
00284 {
00285 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00286 }
00287 }
00288
00289 void PoseDisplay::unsubscribe()
00290 {
00291 sub_.unsubscribe();
00292 }
00293
00294 void PoseDisplay::onEnable()
00295 {
00296 setVisibility();
00297
00298 subscribe();
00299 }
00300
00301 void PoseDisplay::onDisable()
00302 {
00303 unsubscribe();
00304 clear();
00305 scene_node_->setVisible( false );
00306 }
00307
00308 void PoseDisplay::createShapeProperties()
00309 {
00310 if (!property_manager_)
00311 {
00312 return;
00313 }
00314
00315 property_manager_->deleteProperty(shape_category_.lock());
00316 shape_category_ = property_manager_->createCategory("Shape Properties", property_prefix_, parent_category_, this);
00317
00318 switch (current_shape_)
00319 {
00320 case Arrow:
00321 {
00322 color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PoseDisplay::getColor, this ),
00323 boost::bind( &PoseDisplay::setColor, this, _1 ), shape_category_, this );
00324 setPropertyHelpText(color_property_, "Color to draw the arrow.");
00325 alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &PoseDisplay::getAlpha, this ),
00326 boost::bind( &PoseDisplay::setAlpha, this, _1 ), shape_category_, this );
00327 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the arrow.");
00328 FloatPropertyPtr float_prop = alpha_property_.lock();
00329 float_prop->setMin(0.0);
00330 float_prop->setMax(1.0);
00331
00332 shaft_length_property_ = property_manager_->createProperty<FloatProperty>( "Shaft Length", property_prefix_, boost::bind( &PoseDisplay::getShaftLength, this ),
00333 boost::bind( &PoseDisplay::setShaftLength, this, _1 ), shape_category_, this );
00334 setPropertyHelpText(shaft_length_property_, "Length of the arrow's shaft, in meters.");
00335 shaft_radius_property_ = property_manager_->createProperty<FloatProperty>( "Shaft Radius", property_prefix_, boost::bind( &PoseDisplay::getShaftRadius, this ),
00336 boost::bind( &PoseDisplay::setShaftRadius, this, _1 ), shape_category_, this );
00337 setPropertyHelpText(shaft_radius_property_, "Radius of the arrow's shaft, in meters.");
00338 head_length_property_ = property_manager_->createProperty<FloatProperty>( "Head Length", property_prefix_, boost::bind( &PoseDisplay::getHeadLength, this ),
00339 boost::bind( &PoseDisplay::setHeadLength, this, _1 ), shape_category_, this );
00340 setPropertyHelpText(head_length_property_, "Length of the arrow's head, in meters.");
00341 head_radius_property_ = property_manager_->createProperty<FloatProperty>( "Head Radius", property_prefix_, boost::bind( &PoseDisplay::getHeadRadius, this ),
00342 boost::bind( &PoseDisplay::setHeadRadius, this, _1 ), shape_category_, this );
00343 setPropertyHelpText(head_radius_property_, "Radius of the arrow's head, in meters.");
00344 }
00345 break;
00346 case Axes:
00347 axes_length_property_ = property_manager_->createProperty<FloatProperty>( "Axes Length", property_prefix_, boost::bind( &PoseDisplay::getAxesLength, this ),
00348 boost::bind( &PoseDisplay::setAxesLength, this, _1 ), shape_category_, this );
00349 setPropertyHelpText(axes_length_property_, "Length of each axis, in meters.");
00350 axes_radius_property_ = property_manager_->createProperty<FloatProperty>( "Axes Radius", property_prefix_, boost::bind( &PoseDisplay::getAxesRadius, this ),
00351 boost::bind( &PoseDisplay::setAxesRadius, this, _1 ), shape_category_, this );
00352 setPropertyHelpText(axes_radius_property_, "Radius of each axis, in meters.");
00353 break;
00354 }
00355
00356 }
00357
00358 void PoseDisplay::createProperties()
00359 {
00360 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PoseDisplay::getTopic, this ),
00361 boost::bind( &PoseDisplay::setTopic, this, _1 ), parent_category_, this );
00362 setPropertyHelpText(topic_property_, "geometry_msgs::PoseStamped topic to subscribe to.");
00363 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00364 topic_prop->setMessageType(ros::message_traits::datatype<geometry_msgs::PoseStamped>());
00365
00366 shape_property_ = property_manager_->createProperty<EnumProperty>( "Shape", property_prefix_, boost::bind( &PoseDisplay::getShape, this ),
00367 boost::bind( &PoseDisplay::setShape, this, _1 ), parent_category_, this );
00368 setPropertyHelpText(shape_property_, "Shape to display the pose as.");
00369 EnumPropertyPtr enum_prop = shape_property_.lock();
00370 enum_prop->addOption( "Arrow", Arrow );
00371 enum_prop->addOption( "Axes", Axes );
00372
00373 createShapeProperties();
00374 }
00375
00376 void PoseDisplay::fixedFrameChanged()
00377 {
00378 tf_filter_->setTargetFrame( fixed_frame_ );
00379 clear();
00380 }
00381
00382 void PoseDisplay::update(float wall_dt, float ros_dt)
00383 {
00384 }
00385
00386 void PoseDisplay::incomingMessage( const geometry_msgs::PoseStamped::ConstPtr& message )
00387 {
00388 ++messages_received_;
00389
00390 if (!validateFloats(*message))
00391 {
00392 setStatus(status_levels::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00393 return;
00394 }
00395
00396 {
00397 std::stringstream ss;
00398 ss << messages_received_ << " messages received";
00399 setStatus(status_levels::Ok, "Topic", ss.str());
00400 }
00401
00402 Ogre::Vector3 position;
00403 Ogre::Quaternion orientation;
00404 if (!vis_manager_->getFrameManager()->transform(message->header, message->pose, position, orientation))
00405 {
00406 ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'", name_.c_str(), message->header.frame_id.c_str(), fixed_frame_.c_str() );
00407 }
00408
00409 scene_node_->setPosition( position );
00410 scene_node_->setOrientation( orientation );
00411
00412 latest_message_ = message;
00413 coll_handler_->setMessage(message);
00414 setVisibility();
00415
00416 causeRender();
00417 }
00418
00419 void PoseDisplay::reset()
00420 {
00421 Display::reset();
00422 clear();
00423 }
00424
00425 }