interactive_marker_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, 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  * Author: David Gossow
00030  */
00031 
00032 #include "interactive_markers/interactive_marker_server.h"
00033 
00034 #include <visualization_msgs/InteractiveMarkerInit.h>
00035 
00036 #include <boost/bind.hpp>
00037 #include <boost/make_shared.hpp>
00038 
00039 namespace interactive_markers
00040 {
00041 
00042 InteractiveMarkerServer::InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id, bool spin_thread ) :
00043     topic_ns_(topic_ns),
00044     seq_num_(0)
00045 {
00046   if ( spin_thread )
00047   {
00048     // if we're spinning our own thread, we'll also need our own callback queue
00049     node_handle_.setCallbackQueue( &callback_queue_ );
00050   }
00051 
00052   if (!server_id.empty())
00053   {
00054     server_id_ = ros::this_node::getName() + "/" + server_id;
00055   }
00056   else
00057   {
00058     server_id_ = ros::this_node::getName();
00059   }
00060 
00061   std::string update_topic = topic_ns + "/update";
00062   std::string init_topic = update_topic + "_full";
00063   std::string feedback_topic = topic_ns + "/feedback";
00064 
00065   init_pub_ = node_handle_.advertise<visualization_msgs::InteractiveMarkerInit>( init_topic, 100, true );
00066   update_pub_ = node_handle_.advertise<visualization_msgs::InteractiveMarkerUpdate>( update_topic, 100 );
00067   feedback_sub_ = node_handle_.subscribe( feedback_topic, 100, &InteractiveMarkerServer::processFeedback, this );
00068 
00069   keep_alive_timer_ =  node_handle_.createTimer(ros::Duration(0.5f), boost::bind( &InteractiveMarkerServer::keepAlive, this ) );
00070 
00071   if ( spin_thread )
00072   {
00073     need_to_terminate_ = false;
00074     spin_thread_.reset( new boost::thread(boost::bind(&InteractiveMarkerServer::spinThread, this)) );
00075   }
00076 
00077   publishInit();
00078 }
00079 
00080 
00081 InteractiveMarkerServer::~InteractiveMarkerServer()
00082 {
00083   if (spin_thread_.get())
00084   {
00085     need_to_terminate_ = true;
00086     spin_thread_->join();
00087   }
00088 
00089   if ( node_handle_.ok() )
00090   {
00091     clear();
00092     applyChanges();
00093   }
00094 }
00095 
00096 
00097 void InteractiveMarkerServer::spinThread()
00098 {
00099   while (node_handle_.ok())
00100   {
00101     if (need_to_terminate_)
00102     {
00103       break;
00104     }
00105     callback_queue_.callAvailable(ros::WallDuration(0.033f));
00106   }
00107 }
00108 
00109 
00110 void InteractiveMarkerServer::applyChanges()
00111 {
00112   boost::recursive_mutex::scoped_lock lock( mutex_ );
00113 
00114   if ( pending_updates_.empty() )
00115   {
00116     return;
00117   }
00118 
00119   M_UpdateContext::iterator update_it;
00120 
00121   visualization_msgs::InteractiveMarkerUpdate update;
00122   update.type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00123 
00124   update.markers.reserve( marker_contexts_.size() );
00125   update.poses.reserve( marker_contexts_.size() );
00126   update.erases.reserve( marker_contexts_.size() );
00127 
00128   for ( update_it = pending_updates_.begin(); update_it != pending_updates_.end(); update_it++ )
00129   {
00130     M_MarkerContext::iterator marker_context_it = marker_contexts_.find( update_it->first );
00131 
00132     switch ( update_it->second.update_type )
00133     {
00134       case UpdateContext::FULL_UPDATE:
00135       {
00136         if ( marker_context_it == marker_contexts_.end() )
00137         {
00138           ROS_DEBUG("Creating new context for %s", update_it->first.c_str());
00139           // create a new int_marker context
00140           marker_context_it = marker_contexts_.insert( std::make_pair( update_it->first, MarkerContext() ) ).first;
00141           // copy feedback cbs, in case they have been set before the marker context was created
00142           marker_context_it->second.default_feedback_cb = update_it->second.default_feedback_cb;
00143           marker_context_it->second.feedback_cbs = update_it->second.feedback_cbs;
00144         }
00145 
00146         marker_context_it->second.int_marker = update_it->second.int_marker;
00147 
00148         update.markers.push_back( marker_context_it->second.int_marker );
00149         break;
00150       }
00151 
00152       case UpdateContext::POSE_UPDATE:
00153       {
00154         if ( marker_context_it == marker_contexts_.end() )
00155         {
00156           ROS_ERROR( "Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface." );
00157         }
00158         else
00159         {
00160           marker_context_it->second.int_marker.pose = update_it->second.int_marker.pose;
00161           marker_context_it->second.int_marker.header = update_it->second.int_marker.header;
00162 
00163           visualization_msgs::InteractiveMarkerPose pose_update;
00164           pose_update.header = marker_context_it->second.int_marker.header;
00165           pose_update.pose = marker_context_it->second.int_marker.pose;
00166           pose_update.name = marker_context_it->second.int_marker.name;
00167           update.poses.push_back( pose_update );
00168         }
00169         break;
00170       }
00171 
00172       case UpdateContext::ERASE:
00173       {
00174         if ( marker_context_it != marker_contexts_.end() )
00175         {
00176           marker_contexts_.erase( update_it->first );
00177           update.erases.push_back( update_it->first );
00178         }
00179         break;
00180       }
00181     }
00182   }
00183 
00184   seq_num_++;
00185 
00186   publish( update );
00187   publishInit();
00188   pending_updates_.clear();
00189 }
00190 
00191 
00192 bool InteractiveMarkerServer::erase( const std::string &name )
00193 {
00194   boost::recursive_mutex::scoped_lock lock( mutex_ );
00195 
00196   pending_updates_[name].update_type = UpdateContext::ERASE;
00197   return true;
00198 }
00199 
00200 void InteractiveMarkerServer::clear()
00201 {
00202   boost::recursive_mutex::scoped_lock lock( mutex_ );
00203 
00204   // erase all markers
00205   pending_updates_.clear();
00206   M_MarkerContext::iterator it;
00207   for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ )
00208   {
00209     pending_updates_[it->first].update_type = UpdateContext::ERASE;
00210   }
00211 }
00212 
00213 
00214 bool InteractiveMarkerServer::empty() const
00215 {
00216   return marker_contexts_.empty();
00217 }
00218 
00219 
00220 std::size_t InteractiveMarkerServer::size() const
00221 {
00222   return marker_contexts_.size();
00223 }
00224 
00225 
00226 bool InteractiveMarkerServer::setPose( const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
00227 {
00228   boost::recursive_mutex::scoped_lock lock( mutex_ );
00229 
00230   M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
00231   M_UpdateContext::iterator update_it = pending_updates_.find( name );
00232 
00233   // if there's no marker and no pending addition for it, we can't update the pose
00234   if ( marker_context_it == marker_contexts_.end() &&
00235       ( update_it == pending_updates_.end() || update_it->second.update_type != UpdateContext::FULL_UPDATE ) )
00236   {
00237     return false;
00238   }
00239 
00240   // keep the old header
00241   if ( header.frame_id.empty() )
00242   {
00243     if ( marker_context_it != marker_contexts_.end() )
00244     {
00245       doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header );
00246     }
00247     else if ( update_it != pending_updates_.end() )
00248     {
00249       doSetPose( update_it, name, pose, update_it->second.int_marker.header );
00250     }
00251     else
00252     {
00253       BOOST_ASSERT_MSG(false, "Marker does not exist and there is no pending creation.");
00254       return false;
00255     }
00256   }
00257   else
00258   {
00259     doSetPose( update_it, name, pose, header );
00260   }
00261   return true;
00262 }
00263 
00264 bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type  )
00265 {
00266   boost::recursive_mutex::scoped_lock lock( mutex_ );
00267 
00268   M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
00269   M_UpdateContext::iterator update_it = pending_updates_.find( name );
00270 
00271   if ( marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end() )
00272   {
00273     return false;
00274   }
00275 
00276   // we need to overwrite both the callbacks for the actual marker
00277   // and the update, if there's any
00278 
00279   if ( marker_context_it != marker_contexts_.end() )
00280   {
00281     // the marker exists, so we can just overwrite the existing callbacks
00282     if ( feedback_type == DEFAULT_FEEDBACK_CB )
00283     {
00284       marker_context_it->second.default_feedback_cb = feedback_cb;
00285     }
00286     else
00287     {
00288       if ( feedback_cb )
00289       {
00290         marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb;
00291       }
00292       else
00293       {
00294         marker_context_it->second.feedback_cbs.erase( feedback_type );
00295       }
00296     }
00297   }
00298 
00299   if ( update_it != pending_updates_.end() )
00300   {
00301     if ( feedback_type == DEFAULT_FEEDBACK_CB )
00302     {
00303       update_it->second.default_feedback_cb = feedback_cb;
00304     }
00305     else
00306     {
00307       if ( feedback_cb )
00308       {
00309         update_it->second.feedback_cbs[feedback_type] = feedback_cb;
00310       }
00311       else
00312       {
00313         update_it->second.feedback_cbs.erase( feedback_type );
00314       }
00315     }
00316   }
00317   return true;
00318 }
00319 
00320 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker )
00321 {
00322   boost::recursive_mutex::scoped_lock lock( mutex_ );
00323 
00324   M_UpdateContext::iterator update_it = pending_updates_.find( int_marker.name );
00325   if ( update_it == pending_updates_.end() )
00326   {
00327     update_it = pending_updates_.insert( std::make_pair( int_marker.name, UpdateContext() ) ).first;
00328   }
00329 
00330   update_it->second.update_type = UpdateContext::FULL_UPDATE;
00331   update_it->second.int_marker = int_marker;
00332 }
00333 
00334 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker,
00335     FeedbackCallback feedback_cb, uint8_t feedback_type)
00336 {
00337   insert( int_marker );
00338 
00339   setCallback( int_marker.name, feedback_cb, feedback_type  );
00340 }
00341 
00342 bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const
00343 {
00344   boost::recursive_mutex::scoped_lock lock( mutex_ );
00345 
00346   M_UpdateContext::const_iterator update_it = pending_updates_.find( name );
00347 
00348   if ( update_it == pending_updates_.end() )
00349   {
00350     M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
00351     if ( marker_context_it == marker_contexts_.end() )
00352     {
00353       return false;
00354     }
00355 
00356     int_marker = marker_context_it->second.int_marker;
00357     return true;
00358   }
00359 
00360   // if there's an update pending, we'll have to account for that
00361   switch ( update_it->second.update_type )
00362   {
00363     case UpdateContext::ERASE:
00364       return false;
00365 
00366     case UpdateContext::POSE_UPDATE:
00367     {
00368       M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
00369       if ( marker_context_it == marker_contexts_.end() )
00370       {
00371         return false;
00372       }
00373       int_marker = marker_context_it->second.int_marker;
00374       int_marker.pose = update_it->second.int_marker.pose;
00375       return true;
00376     }
00377 
00378     case UpdateContext::FULL_UPDATE:
00379       int_marker = update_it->second.int_marker;
00380       return true;
00381   }
00382 
00383   return false;
00384 }
00385 
00386 void InteractiveMarkerServer::publishInit()
00387 {
00388   boost::recursive_mutex::scoped_lock lock( mutex_ );
00389 
00390   visualization_msgs::InteractiveMarkerInit init;
00391   init.server_id = server_id_;
00392   init.seq_num = seq_num_;
00393   init.markers.reserve( marker_contexts_.size() );
00394 
00395   M_MarkerContext::iterator it;
00396   for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ )
00397   {
00398     ROS_DEBUG( "Publishing %s", it->second.int_marker.name.c_str() );
00399     init.markers.push_back( it->second.int_marker );
00400   }
00401 
00402   init_pub_.publish( init );
00403 }
00404 
00405 void InteractiveMarkerServer::processFeedback( const FeedbackConstPtr& feedback )
00406 {
00407   boost::recursive_mutex::scoped_lock lock( mutex_ );
00408 
00409   M_MarkerContext::iterator marker_context_it = marker_contexts_.find( feedback->marker_name );
00410 
00411   // ignore feedback for non-existing markers
00412   if ( marker_context_it == marker_contexts_.end() )
00413   {
00414     return;
00415   }
00416 
00417   MarkerContext &marker_context = marker_context_it->second;
00418 
00419   // if two callers try to modify the same marker, reject (timeout= 1 sec)
00420   if ( marker_context.last_client_id != feedback->client_id &&
00421       (ros::Time::now() - marker_context.last_feedback).toSec() < 1.0 )
00422   {
00423     ROS_DEBUG( "Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() );
00424     return;
00425   }
00426 
00427   marker_context.last_feedback = ros::Time::now();
00428   marker_context.last_client_id = feedback->client_id;
00429 
00430   if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE )
00431   {
00432     if ( marker_context.int_marker.header.stamp == ros::Time(0) )
00433     {
00434       // keep the old header
00435       doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, marker_context.int_marker.header );
00436     }
00437     else
00438     {
00439       doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, feedback->header );
00440     }
00441   }
00442 
00443   // call feedback handler
00444   boost::unordered_map<uint8_t,FeedbackCallback>::iterator feedback_cb_it = marker_context.feedback_cbs.find( feedback->event_type );
00445   if ( feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second )
00446   {
00447     // call type-specific callback
00448     feedback_cb_it->second( feedback );
00449   }
00450   else if ( marker_context.default_feedback_cb )
00451   {
00452     // call default callback
00453     marker_context.default_feedback_cb(  feedback );
00454   }
00455 }
00456 
00457 
00458 void InteractiveMarkerServer::keepAlive()
00459 {
00460   visualization_msgs::InteractiveMarkerUpdate empty_update;
00461   empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
00462   publish( empty_update );
00463 }
00464 
00465 
00466 void InteractiveMarkerServer::publish( visualization_msgs::InteractiveMarkerUpdate &update )
00467 {
00468   update.server_id = server_id_;
00469   update.seq_num = seq_num_;
00470   update_pub_.publish( update );
00471 }
00472 
00473 
00474 void InteractiveMarkerServer::doSetPose( M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
00475 {
00476   if ( update_it == pending_updates_.end() )
00477   {
00478     update_it = pending_updates_.insert( std::make_pair( name, UpdateContext() ) ).first;
00479     update_it->second.update_type = UpdateContext::POSE_UPDATE;
00480   }
00481   else if ( update_it->second.update_type != UpdateContext::FULL_UPDATE )
00482   {
00483     update_it->second.update_type = UpdateContext::POSE_UPDATE;
00484   }
00485 
00486   update_it->second.int_marker.pose = pose;
00487   update_it->second.int_marker.header = header;
00488   ROS_DEBUG( "Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z );
00489 }
00490 
00491 
00492 }


interactive_markers
Author(s): David Gossow
autogenerated on Sat Aug 27 2016 03:37:45