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
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
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
00140 marker_context_it = marker_contexts_.insert( std::make_pair( update_it->first, MarkerContext() ) ).first;
00141
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
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
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
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
00277
00278
00279 if ( marker_context_it != marker_contexts_.end() )
00280 {
00281
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
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
00412 if ( marker_context_it == marker_contexts_.end() )
00413 {
00414 return;
00415 }
00416
00417 MarkerContext &marker_context = marker_context_it->second;
00418
00419
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
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
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
00448 feedback_cb_it->second( feedback );
00449 }
00450 else if ( marker_context.default_feedback_cb )
00451 {
00452
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 }