pose_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 #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   // Arrow points in -Z direction, so rotate the orientation before display.
00117   // TODO: is it safe to change Arrow to point in +X direction?
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 } // namespace rviz


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32