interactive_marker_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 "interactive_marker_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property_manager.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/selection/selection_manager.h"
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/validate_floats.h"
00037 
00038 
00039 #include <tf/transform_listener.h>
00040 
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 
00044 namespace rviz
00045 {
00046 
00048 bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
00049 {
00050   bool valid = true;
00051   valid = valid && validateFloats(msg.pose);
00052   valid = valid && validateFloats(msg.scale);
00053   for ( unsigned c=0; c<msg.controls.size(); c++)
00054   {
00055     valid = valid && validateFloats( msg.controls[c].orientation );
00056     for ( unsigned m=0; m<msg.controls[c].markers.size(); m++ )
00057     {
00058       valid = valid && validateFloats(msg.controls[c].markers[m].pose);
00059       valid = valid && validateFloats(msg.controls[c].markers[m].scale);
00060       valid = valid && validateFloats(msg.controls[c].markers[m].color);
00061       valid = valid && validateFloats(msg.controls[c].markers[m].points);
00062     }
00063   }
00064   return valid;
00065 }
00067 
00068 
00069 
00070 InteractiveMarkerDisplay::InteractiveMarkerDisplay()
00071   : Display()
00072   , im_client_( this )
00073   , show_descriptions_(true)
00074   , show_tool_tips_(true)
00075   , show_axes_(false)
00076 {
00077 }
00078 
00079 void InteractiveMarkerDisplay::onInitialize()
00080 {
00081   tf_filter_ = new tf::MessageFilter<visualization_msgs::InteractiveMarker>(*vis_manager_->getTFClient(), "", 100, update_nh_);
00082   tf_pose_filter_ = new tf::MessageFilter<visualization_msgs::InteractiveMarkerPose>(*vis_manager_->getTFClient(), "", 100, update_nh_);
00083   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00084 
00085   //message flow: incomingMarkerUpdate(..) -> tf_filter_ -> tfMarkerSuccess(..) -> message_queue_ -> update(..)
00086 
00087   tf_filter_->registerCallback(boost::bind(&InteractiveMarkerDisplay::tfMarkerSuccess, this, _1));
00088   tf_filter_->registerFailureCallback(boost::bind(&InteractiveMarkerDisplay::tfMarkerFail, this, _1, _2));
00089   tf_pose_filter_->registerCallback(boost::bind(&InteractiveMarkerDisplay::tfPoseSuccess, this, _1));
00090   tf_pose_filter_->registerFailureCallback(boost::bind(&InteractiveMarkerDisplay::tfPoseFail, this, _1, _2));
00091 
00092   client_id_ = ros::this_node::getName() + "/" + name_;
00093 }
00094 
00095 InteractiveMarkerDisplay::~InteractiveMarkerDisplay()
00096 {
00097   unsubscribe();
00098   scene_manager_->destroySceneNode( scene_node_ );
00099   delete tf_filter_;
00100   delete tf_pose_filter_;
00101 }
00102 
00103 void InteractiveMarkerDisplay::onEnable()
00104 {
00105   subscribe();
00106 }
00107 
00108 void InteractiveMarkerDisplay::onDisable()
00109 {
00110   unsubscribe();
00111   tf_filter_->clear();
00112   tf_pose_filter_->clear();
00113 }
00114 
00115 void InteractiveMarkerDisplay::hideVisible() 
00116 {
00117   for (M_StringToInteractiveMarkerPtr::iterator it = interactive_markers_.begin();
00118        it != interactive_markers_.end(); it++)
00119   {
00120     it->second->hideVisible();
00121   }  
00122 }
00123 
00124 void InteractiveMarkerDisplay::restoreVisible() 
00125 {
00126   for (M_StringToInteractiveMarkerPtr::iterator it = interactive_markers_.begin();
00127        it != interactive_markers_.end(); it++)
00128   {
00129     it->second->restoreVisible();
00130   }  
00131 }
00132 
00133 void InteractiveMarkerDisplay::setMarkerUpdateTopic(const std::string& topic)
00134 {
00135   if ( marker_update_topic_.empty() && topic.empty() )
00136   {
00137     return;
00138   }
00139   unsubscribe();
00140   marker_update_topic_ = topic;
00141   subscribe();
00142   propertyChanged(marker_update_topic_property_);
00143 }
00144 
00145 void InteractiveMarkerDisplay::subscribe()
00146 {
00147   if ( !isEnabled() )
00148   {
00149     return;
00150   }
00151 
00152   marker_update_sub_.shutdown();
00153   num_publishers_ = 0;
00154 
00155   try
00156   {
00157     if ( !marker_update_topic_.empty() )
00158     {
00159       ROS_DEBUG( "Subscribing to %s", marker_update_topic_.c_str() );
00160       marker_update_sub_ = update_nh_.subscribe(marker_update_topic_, 100, &InteractiveMarkerClient::processMarkerUpdate, &im_client_);
00161     }
00162 
00163     setStatus(status_levels::Ok, "Topic", "OK");
00164   }
00165   catch (ros::Exception& e)
00166   {
00167     setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00168   }
00169 
00170   im_client_.clear();
00171 }
00172 
00173 bool InteractiveMarkerDisplay::subscribeToInit()
00174 {
00175   bool result = false;
00176 
00177   ROS_DEBUG( "subscribeToInit()" );
00178   try
00179   {
00180     if ( !marker_update_topic_.empty() )
00181     {
00182       std::string init_topic = marker_update_topic_ + "_full";
00183       ROS_DEBUG( "Subscribing to %s", init_topic.c_str() );
00184       marker_init_sub_ = update_nh_.subscribe(init_topic, 100, &InteractiveMarkerClient::processMarkerInit, &im_client_);
00185       result = true;
00186     }
00187 
00188     setStatus(status_levels::Ok, "InitTopic", "OK");
00189   }
00190   catch (ros::Exception& e)
00191   {
00192     setStatus(status_levels::Error, "InitTopic", std::string("Error subscribing: ") + e.what());
00193   }
00194   return result;
00195 }
00196 
00197 void InteractiveMarkerDisplay::clearMarkers()
00198 {
00199   interactive_markers_.clear();
00200   tf_filter_->clear();
00201   tf_pose_filter_->clear();
00202 }
00203 
00204 void InteractiveMarkerDisplay::unsubscribe()
00205 {
00206   marker_update_sub_.shutdown();
00207   marker_init_sub_.shutdown();
00208   clearMarkers();
00209   im_client_.unsubscribedFromInit();
00210 }
00211 
00212 void InteractiveMarkerDisplay::unsubscribeFromInit()
00213 {
00214   marker_init_sub_.shutdown();
00215 }
00216 
00217 void InteractiveMarkerDisplay::processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers,
00218                                                      const std::vector<visualization_msgs::InteractiveMarkerPose>* poses,
00219                                                      const std::vector<std::string>* erases )
00220 {
00221   std::set<std::string> names;
00222 
00223   if( markers != NULL )
00224   {
00225     std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_it = markers->begin();
00226     std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_end = markers->end();
00227     for (; marker_it != marker_end; ++marker_it)
00228     {
00229       if ( !names.insert( marker_it->name ).second )
00230       {
00231         setStatus(status_levels::Error, "Marker array", "The name '" + marker_it->name + "' was used multiple times.");
00232       }
00233 
00234       visualization_msgs::InteractiveMarker::ConstPtr marker_ptr(new visualization_msgs::InteractiveMarker(*marker_it));
00235 
00236       if ( marker_it->header.stamp == ros::Time(0) )
00237       {
00238         // bypass tf filter
00239         updateMarker( marker_ptr );
00240         vis_manager_->queueRender();
00241       }
00242       else
00243       {
00244         ROS_DEBUG("Forwarding %s to tf filter.", marker_it->name.c_str());
00245         tf_filter_->add( marker_ptr );
00246       }
00247     }
00248   }
00249 
00250   if( poses != NULL )
00251   {
00252     // pipe all pose updates into tf filter
00253     std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_it = poses->begin();
00254     std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_end = poses->end();
00255 
00256     for (; pose_it != pose_end; ++pose_it)
00257     {
00258       if ( !names.insert( pose_it->name ).second )
00259       {
00260         setStatus(status_levels::Error, "Marker array", "The name '" + pose_it->name + "' was used multiple times.");
00261       }
00262 
00263       visualization_msgs::InteractiveMarkerPose::ConstPtr pose_ptr(new visualization_msgs::InteractiveMarkerPose(*pose_it));
00264 
00265       if ( pose_it->header.stamp == ros::Time(0) )
00266       {
00267         updatePose( pose_ptr );
00268         vis_manager_->queueRender();
00269       }
00270       else
00271       {
00272         ROS_DEBUG("Forwarding pose for %s to tf filter.", pose_it->name.c_str());
00273         tf_pose_filter_->add( pose_ptr );
00274       }
00275     }
00276   }
00277 
00278   if( erases != NULL )
00279   {
00280     // erase markers
00281     std::vector<std::string>::const_iterator erase_it = erases->begin();
00282     std::vector<std::string>::const_iterator erase_end = erases->end();
00283 
00284     for (; erase_it != erase_end; ++erase_it)
00285     {
00286       interactive_markers_.erase( *erase_it );
00287     }
00288   }
00289 }
00290 
00291 void InteractiveMarkerDisplay::tfMarkerSuccess( const visualization_msgs::InteractiveMarker::ConstPtr& marker )
00292 {
00293   ROS_DEBUG("Queueing %s", marker->name.c_str());
00294   boost::mutex::scoped_lock lock(queue_mutex_);
00295   marker_queue_.push_back(marker);
00296   vis_manager_->queueRender();
00297 }
00298 
00299 void InteractiveMarkerDisplay::tfMarkerFail(const visualization_msgs::InteractiveMarker::ConstPtr& marker, tf::FilterFailureReason reason)
00300 {
00301   std::string error = FrameManager::instance()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
00302   setStatus( status_levels::Error, marker->name, error);
00303 }
00304 
00305 void InteractiveMarkerDisplay::tfPoseSuccess(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose)
00306 {
00307   ROS_DEBUG("Queueing pose for %s", marker_pose->name.c_str());
00308   boost::mutex::scoped_lock lock(queue_mutex_);
00309   pose_queue_.push_back(marker_pose);
00310   vis_manager_->queueRender();
00311 }
00312 
00313 void InteractiveMarkerDisplay::tfPoseFail(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose, tf::FilterFailureReason reason)
00314 {
00315   std::string error = FrameManager::instance()->discoverFailureReason(
00316       marker_pose->header.frame_id, marker_pose->header.stamp,
00317       marker_pose->__connection_header ? (*marker_pose->__connection_header)["callerid"] : "unknown", reason);
00318   setStatus( status_levels::Error, marker_pose->name, error);
00319 }
00320 
00321 
00322 void InteractiveMarkerDisplay::update(float wall_dt, float ros_dt)
00323 {
00324   //detect if all servers have shut down
00325   if ( !im_client_.isPublisherListEmpty() )
00326   {
00327     // weak detection of server shutdown
00328     unsigned num_pub = marker_update_sub_.getNumPublishers();
00329     if ( num_pub < num_publishers_ )
00330     {
00331       reset();
00332     }
00333     else
00334     {
00335       num_publishers_ = num_pub;
00336     }
00337 
00338     im_client_.flagLateConnections();
00339   }
00340 
00341   V_InteractiveMarkerMessage local_marker_queue;
00342   V_InteractiveMarkerPoseMessage local_pose_queue;
00343 
00344   // the queue is accessed from another thread, so we need to lock it
00345   // and swap it's contents to a copy
00346   {
00347     boost::mutex::scoped_lock lock(queue_mutex_);
00348     local_marker_queue.swap( marker_queue_ );
00349     local_pose_queue.swap( pose_queue_ );
00350   }
00351 
00352   if ( !local_marker_queue.empty() )
00353   {
00354     V_InteractiveMarkerMessage::iterator message_it = local_marker_queue.begin();
00355     V_InteractiveMarkerMessage::iterator message_end = local_marker_queue.end();
00356     for ( ; message_it != message_end; ++message_it )
00357     {
00358       updateMarker( *message_it );
00359     }
00360   }
00361 
00362   if ( !local_pose_queue.empty() )
00363   {
00364     V_InteractiveMarkerPoseMessage::iterator message_it = local_pose_queue.begin();
00365     V_InteractiveMarkerPoseMessage::iterator message_end = local_pose_queue.end();
00366     for ( ; message_it != message_end; ++message_it )
00367     {
00368       updatePose( *message_it );
00369     }
00370   }
00371 
00372   M_StringToInteractiveMarkerPtr::iterator it;
00373   for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00374   {
00375     it->second->update( wall_dt );
00376   }
00377 }
00378 
00379 void InteractiveMarkerDisplay::updateMarker( visualization_msgs::InteractiveMarker::ConstPtr& marker )
00380 {
00381   if ( !validateFloats( *marker ) )
00382   {
00383     setStatus( status_levels::Error, marker->name, "Message contains invalid floats!" );
00384     return;
00385   }
00386   ROS_DEBUG("Processing interactive marker '%s'. %d", marker->name.c_str(), (int)marker->controls.size() );
00387 
00388   std::map< std::string, InteractiveMarkerPtr >::iterator int_marker_entry = interactive_markers_.find( marker->name );
00389 
00390   std::string topic = marker_update_topic_;
00391 
00392   topic = ros::names::clean( topic );
00393   topic = topic.substr( 0, topic.find_last_of( '/' ) );
00394 
00395   if ( int_marker_entry == interactive_markers_.end() )
00396   {
00397     int_marker_entry = interactive_markers_.insert( std::make_pair(marker->name, InteractiveMarkerPtr ( new InteractiveMarker(this, vis_manager_, topic, client_id_) ) ) ).first;
00398   }
00399 
00400   if ( int_marker_entry->second->processMessage( marker ) )
00401   {
00402     int_marker_entry->second->setShowAxes(show_axes_);
00403     int_marker_entry->second->setShowDescription(show_descriptions_);
00404   }
00405 }
00406 
00407 void InteractiveMarkerDisplay::updatePose( visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose )
00408 {
00409   if ( !validateFloats( marker_pose->pose ) )
00410   {
00411     setStatus( status_levels::Error, marker_pose->name, "Pose message contains invalid floats!" );
00412     return;
00413   }
00414 
00415   std::map< std::string, InteractiveMarkerPtr >::iterator int_marker_entry = interactive_markers_.find( marker_pose->name );
00416 
00417   if ( int_marker_entry != interactive_markers_.end() )
00418   {
00419     int_marker_entry->second->processMessage( marker_pose );
00420   }
00421 }
00422 
00423 void InteractiveMarkerDisplay::fixedFrameChanged()
00424 {
00425   tf_filter_->setTargetFrame( fixed_frame_ );
00426   tf_pose_filter_->setTargetFrame( fixed_frame_ );
00427   reset();
00428 }
00429 
00430 void InteractiveMarkerDisplay::reset()
00431 {
00432   ROS_DEBUG("reset");
00433   Display::reset();
00434   unsubscribe();
00435   subscribe();
00436 }
00437 
00438 void InteractiveMarkerDisplay::createProperties()
00439 {
00440   // interactive marker update topic
00441   marker_update_topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>(
00442       "Update Topic", property_prefix_, boost::bind( &InteractiveMarkerDisplay::getMarkerUpdateTopic, this ),
00443       boost::bind( &InteractiveMarkerDisplay::setMarkerUpdateTopic, this, _1 ), parent_category_, this );
00444 
00445   setPropertyHelpText(marker_update_topic_property_, "visualization_msgs::InteractiveMarkerUpdate topic to subscribe to.");
00446   ROSTopicStringPropertyPtr marker_update_topic_property_locked = marker_update_topic_property_.lock();
00447   marker_update_topic_property_locked->setMessageType(ros::message_traits::datatype<visualization_msgs::InteractiveMarkerUpdate>());
00448 
00449   // display options
00450   show_descriptions_property_ = property_manager_->createProperty<BoolProperty>(
00451       "Show Descriptions", property_prefix_, boost::bind( &InteractiveMarkerDisplay::getShowDescriptions, this ),
00452       boost::bind( &InteractiveMarkerDisplay::setShowDescriptions, this, _1 ), parent_category_, this );
00453 
00454   setPropertyHelpText(show_descriptions_property_, "Whether or not to show the descriptions of each Interactive Marker.");
00455 
00456   show_tool_tips_property_ = property_manager_->createProperty<BoolProperty>(
00457       "Show Tool Tips", property_prefix_, boost::bind( &InteractiveMarkerDisplay::getShowToolTips, this ),
00458       boost::bind( &InteractiveMarkerDisplay::setShowToolTips, this, _1 ), parent_category_, this );
00459 
00460   setPropertyHelpText(show_tool_tips_property_, "Whether or not to show tool tips for the Interactive Marker Controls.");
00461 
00462   show_axes_property_ = property_manager_->createProperty<BoolProperty>(
00463       "Show Axes", property_prefix_, boost::bind( &InteractiveMarkerDisplay::getShowAxes, this ),
00464       boost::bind( &InteractiveMarkerDisplay::setShowAxes, this, _1 ), parent_category_, this );
00465 
00466   setPropertyHelpText(show_axes_property_, "Whether or not to show the axes of each Interactive Marker.");
00467 }
00468 
00469 
00470 void InteractiveMarkerDisplay::setShowDescriptions( bool show )
00471 {
00472   show_descriptions_ = show;
00473 
00474   M_StringToInteractiveMarkerPtr::iterator it;
00475   for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00476   {
00477     it->second->setShowDescription(show);
00478   }
00479 
00480   propertyChanged(show_descriptions_property_);
00481 }
00482 
00483 
00484 void InteractiveMarkerDisplay::setShowToolTips( bool show )
00485 {
00486   show_tool_tips_ = show;
00487 
00488   propertyChanged(show_descriptions_property_);
00489 }
00490 
00491 
00492 void InteractiveMarkerDisplay::setShowAxes( bool show )
00493 {
00494   show_axes_ = show;
00495 
00496   M_StringToInteractiveMarkerPtr::iterator it;
00497   for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00498   {
00499     it->second->setShowAxes(show);
00500   }
00501 
00502   propertyChanged(show_axes_property_);
00503 }
00504 
00505 void InteractiveMarkerDisplay::setStatusOk(const std::string& name, const std::string& text)
00506 {
00507   setStatus( status_levels::Ok, name, text );
00508 }
00509 
00510 void InteractiveMarkerDisplay::setStatusWarn(const std::string& name, const std::string& text)
00511 {
00512   setStatus( status_levels::Warn, name, text );
00513 }
00514 
00515 void InteractiveMarkerDisplay::setStatusError(const std::string& name, const std::string& text)
00516 {
00517   setStatus( status_levels::Error, name, text );
00518 }
00519 
00520 } // namespace rviz


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