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::setPose( const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
00215 {
00216 boost::recursive_mutex::scoped_lock lock( mutex_ );
00217
00218 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
00219 M_UpdateContext::iterator update_it = pending_updates_.find( name );
00220
00221
00222 if ( marker_context_it == marker_contexts_.end() &&
00223 ( update_it == pending_updates_.end() || update_it->second.update_type != UpdateContext::FULL_UPDATE ) )
00224 {
00225 return false;
00226 }
00227
00228
00229 if ( header.frame_id.empty() )
00230 {
00231 if ( marker_context_it != marker_contexts_.end() )
00232 {
00233 doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header );
00234 }
00235 else if ( update_it != pending_updates_.end() )
00236 {
00237 doSetPose( update_it, name, pose, update_it->second.int_marker.header );
00238 }
00239 else
00240 {
00241 BOOST_ASSERT_MSG(false, "Marker does not exist and there is no pending creation.");
00242 return false;
00243 }
00244 }
00245 else
00246 {
00247 doSetPose( update_it, name, pose, header );
00248 }
00249 return true;
00250 }
00251
00252 bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type )
00253 {
00254 boost::recursive_mutex::scoped_lock lock( mutex_ );
00255
00256 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name );
00257 M_UpdateContext::iterator update_it = pending_updates_.find( name );
00258
00259 if ( marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end() )
00260 {
00261 return false;
00262 }
00263
00264
00265
00266
00267 if ( marker_context_it != marker_contexts_.end() )
00268 {
00269
00270 if ( feedback_type == DEFAULT_FEEDBACK_CB )
00271 {
00272 marker_context_it->second.default_feedback_cb = feedback_cb;
00273 }
00274 else
00275 {
00276 if ( feedback_cb )
00277 {
00278 marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb;
00279 }
00280 else
00281 {
00282 marker_context_it->second.feedback_cbs.erase( feedback_type );
00283 }
00284 }
00285 }
00286
00287 if ( update_it != pending_updates_.end() )
00288 {
00289 if ( feedback_type == DEFAULT_FEEDBACK_CB )
00290 {
00291 update_it->second.default_feedback_cb = feedback_cb;
00292 }
00293 else
00294 {
00295 if ( feedback_cb )
00296 {
00297 update_it->second.feedback_cbs[feedback_type] = feedback_cb;
00298 }
00299 else
00300 {
00301 update_it->second.feedback_cbs.erase( feedback_type );
00302 }
00303 }
00304 }
00305 return true;
00306 }
00307
00308 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker )
00309 {
00310 boost::recursive_mutex::scoped_lock lock( mutex_ );
00311
00312 M_UpdateContext::iterator update_it = pending_updates_.find( int_marker.name );
00313 if ( update_it == pending_updates_.end() )
00314 {
00315 update_it = pending_updates_.insert( std::make_pair( int_marker.name, UpdateContext() ) ).first;
00316 }
00317
00318 update_it->second.update_type = UpdateContext::FULL_UPDATE;
00319 update_it->second.int_marker = int_marker;
00320 }
00321
00322 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker,
00323 FeedbackCallback feedback_cb, uint8_t feedback_type)
00324 {
00325 insert( int_marker );
00326
00327 setCallback( int_marker.name, feedback_cb, feedback_type );
00328 }
00329
00330 bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const
00331 {
00332 boost::recursive_mutex::scoped_lock lock( mutex_ );
00333
00334 M_UpdateContext::const_iterator update_it = pending_updates_.find( name );
00335
00336 if ( update_it == pending_updates_.end() )
00337 {
00338 M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
00339 if ( marker_context_it == marker_contexts_.end() )
00340 {
00341 return false;
00342 }
00343
00344 int_marker = marker_context_it->second.int_marker;
00345 return true;
00346 }
00347
00348
00349 switch ( update_it->second.update_type )
00350 {
00351 case UpdateContext::ERASE:
00352 return false;
00353
00354 case UpdateContext::POSE_UPDATE:
00355 {
00356 M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name );
00357 if ( marker_context_it == marker_contexts_.end() )
00358 {
00359 return false;
00360 }
00361 int_marker = marker_context_it->second.int_marker;
00362 int_marker.pose = update_it->second.int_marker.pose;
00363 return true;
00364 }
00365
00366 case UpdateContext::FULL_UPDATE:
00367 int_marker = update_it->second.int_marker;
00368 return true;
00369 }
00370
00371 return false;
00372 }
00373
00374 void InteractiveMarkerServer::publishInit()
00375 {
00376 boost::recursive_mutex::scoped_lock lock( mutex_ );
00377
00378 visualization_msgs::InteractiveMarkerInit init;
00379 init.server_id = server_id_;
00380 init.seq_num = seq_num_;
00381 init.markers.reserve( marker_contexts_.size() );
00382
00383 M_MarkerContext::iterator it;
00384 for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ )
00385 {
00386 ROS_DEBUG( "Publishing %s", it->second.int_marker.name.c_str() );
00387 init.markers.push_back( it->second.int_marker );
00388 }
00389
00390 init_pub_.publish( init );
00391 }
00392
00393 void InteractiveMarkerServer::processFeedback( const FeedbackConstPtr& feedback )
00394 {
00395 boost::recursive_mutex::scoped_lock lock( mutex_ );
00396
00397 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( feedback->marker_name );
00398
00399
00400 if ( marker_context_it == marker_contexts_.end() )
00401 {
00402 return;
00403 }
00404
00405 MarkerContext &marker_context = marker_context_it->second;
00406
00407
00408 if ( marker_context.last_client_id != feedback->client_id &&
00409 (ros::Time::now() - marker_context.last_feedback).toSec() < 1.0 )
00410 {
00411 ROS_DEBUG( "Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() );
00412 return;
00413 }
00414
00415 marker_context.last_feedback = ros::Time::now();
00416 marker_context.last_client_id = feedback->client_id;
00417
00418 if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE )
00419 {
00420 if ( marker_context.int_marker.header.stamp == ros::Time(0) )
00421 {
00422
00423 doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, marker_context.int_marker.header );
00424 }
00425 else
00426 {
00427 doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, feedback->header );
00428 }
00429 }
00430
00431
00432 boost::unordered_map<uint8_t,FeedbackCallback>::iterator feedback_cb_it = marker_context.feedback_cbs.find( feedback->event_type );
00433 if ( feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second )
00434 {
00435
00436 feedback_cb_it->second( feedback );
00437 }
00438 else if ( marker_context.default_feedback_cb )
00439 {
00440
00441 marker_context.default_feedback_cb( feedback );
00442 }
00443 }
00444
00445
00446 void InteractiveMarkerServer::keepAlive()
00447 {
00448 visualization_msgs::InteractiveMarkerUpdate empty_update;
00449 empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
00450 publish( empty_update );
00451 }
00452
00453
00454 void InteractiveMarkerServer::publish( visualization_msgs::InteractiveMarkerUpdate &update )
00455 {
00456 update.server_id = server_id_;
00457 update.seq_num = seq_num_;
00458 update_pub_.publish( update );
00459 }
00460
00461
00462 void InteractiveMarkerServer::doSetPose( M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header )
00463 {
00464 if ( update_it == pending_updates_.end() )
00465 {
00466 update_it = pending_updates_.insert( std::make_pair( name, UpdateContext() ) ).first;
00467 update_it->second.update_type = UpdateContext::POSE_UPDATE;
00468 }
00469 else if ( update_it->second.update_type != UpdateContext::FULL_UPDATE )
00470 {
00471 update_it->second.update_type = UpdateContext::POSE_UPDATE;
00472 }
00473
00474 update_it->second.int_marker.pose = pose;
00475 update_it->second.int_marker.header = header;
00476 ROS_DEBUG( "Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z );
00477 }
00478
00479
00480 }