$search
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 00162 visualization_msgs::InteractiveMarkerPose pose_update; 00163 pose_update.header = marker_context_it->second.int_marker.header; 00164 pose_update.pose = marker_context_it->second.int_marker.pose; 00165 pose_update.name = marker_context_it->second.int_marker.name; 00166 update.poses.push_back( pose_update ); 00167 } 00168 break; 00169 } 00170 00171 case UpdateContext::ERASE: 00172 { 00173 if ( marker_context_it != marker_contexts_.end() ) 00174 { 00175 marker_contexts_.erase( update_it->first ); 00176 update.erases.push_back( update_it->first ); 00177 } 00178 break; 00179 } 00180 } 00181 } 00182 00183 seq_num_++; 00184 00185 publish( update ); 00186 publishInit(); 00187 pending_updates_.clear(); 00188 } 00189 00190 00191 bool InteractiveMarkerServer::erase( const std::string &name ) 00192 { 00193 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00194 00195 pending_updates_[name].update_type = UpdateContext::ERASE; 00196 return true; 00197 } 00198 00199 void InteractiveMarkerServer::clear() 00200 { 00201 // erase all markers 00202 pending_updates_.clear(); 00203 M_MarkerContext::iterator it; 00204 for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ ) 00205 { 00206 erase( it->first ); 00207 } 00208 } 00209 00210 00211 bool InteractiveMarkerServer::setPose( const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header ) 00212 { 00213 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00214 00215 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name ); 00216 M_UpdateContext::iterator update_it = pending_updates_.find( name ); 00217 00218 // if there's no marker and no pending addition for it, we can't update the pose 00219 if ( marker_context_it == marker_contexts_.end() && 00220 ( update_it == pending_updates_.end() || update_it->second.update_type != UpdateContext::FULL_UPDATE ) ) 00221 { 00222 return false; 00223 } 00224 00225 if ( header.frame_id.empty() ) 00226 { 00227 // keep the old header 00228 doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header ); 00229 } 00230 else 00231 { 00232 doSetPose( update_it, name, pose, header ); 00233 } 00234 return true; 00235 } 00236 00237 bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type ) 00238 { 00239 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00240 00241 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name ); 00242 M_UpdateContext::iterator update_it = pending_updates_.find( name ); 00243 00244 if ( marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end() ) 00245 { 00246 return false; 00247 } 00248 00249 // we need to overwrite both the callbacks for the actual marker 00250 // and the update, if there's any 00251 00252 if ( marker_context_it != marker_contexts_.end() ) 00253 { 00254 // the marker exists, so we can just overwrite the existing callbacks 00255 if ( feedback_type == DEFAULT_FEEDBACK_CB ) 00256 { 00257 marker_context_it->second.default_feedback_cb = feedback_cb; 00258 } 00259 else 00260 { 00261 if ( feedback_cb ) 00262 { 00263 marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb; 00264 } 00265 else 00266 { 00267 marker_context_it->second.feedback_cbs.erase( feedback_type ); 00268 } 00269 } 00270 } 00271 00272 if ( update_it != pending_updates_.end() ) 00273 { 00274 if ( feedback_type == DEFAULT_FEEDBACK_CB ) 00275 { 00276 update_it->second.default_feedback_cb = feedback_cb; 00277 } 00278 else 00279 { 00280 if ( feedback_cb ) 00281 { 00282 update_it->second.feedback_cbs[feedback_type] = feedback_cb; 00283 } 00284 else 00285 { 00286 update_it->second.feedback_cbs.erase( feedback_type ); 00287 } 00288 } 00289 } 00290 return true; 00291 } 00292 00293 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker ) 00294 { 00295 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00296 00297 M_UpdateContext::iterator update_it = pending_updates_.find( int_marker.name ); 00298 if ( update_it == pending_updates_.end() ) 00299 { 00300 update_it = pending_updates_.insert( std::make_pair( int_marker.name, UpdateContext() ) ).first; 00301 } 00302 00303 update_it->second.update_type = UpdateContext::FULL_UPDATE; 00304 update_it->second.int_marker = int_marker; 00305 } 00306 00307 void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker, 00308 FeedbackCallback feedback_cb, uint8_t feedback_type) 00309 { 00310 insert( int_marker ); 00311 00312 setCallback( int_marker.name, feedback_cb, feedback_type ); 00313 } 00314 00315 bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const 00316 { 00317 M_UpdateContext::const_iterator update_it = pending_updates_.find( name ); 00318 00319 if ( update_it == pending_updates_.end() ) 00320 { 00321 M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name ); 00322 if ( marker_context_it == marker_contexts_.end() ) 00323 { 00324 return false; 00325 } 00326 00327 int_marker = marker_context_it->second.int_marker; 00328 return true; 00329 } 00330 00331 // if there's an update pending, we'll have to account for that 00332 switch ( update_it->second.update_type ) 00333 { 00334 case UpdateContext::ERASE: 00335 return false; 00336 00337 case UpdateContext::POSE_UPDATE: 00338 { 00339 M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name ); 00340 if ( marker_context_it == marker_contexts_.end() ) 00341 { 00342 return false; 00343 } 00344 int_marker = marker_context_it->second.int_marker; 00345 int_marker.pose = update_it->second.int_marker.pose; 00346 return true; 00347 } 00348 00349 case UpdateContext::FULL_UPDATE: 00350 int_marker = update_it->second.int_marker; 00351 return true; 00352 } 00353 00354 return false; 00355 } 00356 00357 void InteractiveMarkerServer::publishInit() 00358 { 00359 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00360 00361 visualization_msgs::InteractiveMarkerInit init; 00362 init.server_id = server_id_; 00363 init.seq_num = seq_num_; 00364 init.markers.reserve( marker_contexts_.size() ); 00365 00366 M_MarkerContext::iterator it; 00367 for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ ) 00368 { 00369 ROS_DEBUG( "Publishing %s", it->second.int_marker.name.c_str() ); 00370 init.markers.push_back( it->second.int_marker ); 00371 } 00372 00373 init_pub_.publish( init ); 00374 } 00375 00376 void InteractiveMarkerServer::processFeedback( const FeedbackConstPtr& feedback ) 00377 { 00378 boost::recursive_mutex::scoped_lock lock( mutex_ ); 00379 00380 M_MarkerContext::iterator marker_context_it = marker_contexts_.find( feedback->marker_name ); 00381 00382 // ignore feedback for non-existing markers 00383 if ( marker_context_it == marker_contexts_.end() ) 00384 { 00385 return; 00386 } 00387 00388 MarkerContext &marker_context = marker_context_it->second; 00389 00390 // if two callers try to modify the same marker, reject (timeout= 1 sec) 00391 if ( marker_context.last_client_id != feedback->client_id && 00392 (ros::Time::now() - marker_context.last_feedback).toSec() < 1.0 ) 00393 { 00394 ROS_DEBUG( "Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() ); 00395 return; 00396 } 00397 00398 marker_context.last_feedback = ros::Time::now(); 00399 marker_context.last_client_id = feedback->client_id; 00400 00401 if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE ) 00402 { 00403 if ( marker_context.int_marker.header.stamp == ros::Time(0) ) 00404 { 00405 // keep the old header 00406 doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, marker_context.int_marker.header ); 00407 } 00408 else 00409 { 00410 doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, feedback->header ); 00411 } 00412 } 00413 00414 // call feedback handler 00415 boost::unordered_map<uint8_t,FeedbackCallback>::iterator feedback_cb_it = marker_context.feedback_cbs.find( feedback->event_type ); 00416 if ( feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second ) 00417 { 00418 // call type-specific callback 00419 feedback_cb_it->second( feedback ); 00420 } 00421 else if ( marker_context.default_feedback_cb ) 00422 { 00423 // call default callback 00424 marker_context.default_feedback_cb( feedback ); 00425 } 00426 } 00427 00428 00429 void InteractiveMarkerServer::keepAlive() 00430 { 00431 visualization_msgs::InteractiveMarkerUpdate empty_update; 00432 empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE; 00433 publish( empty_update ); 00434 } 00435 00436 00437 void InteractiveMarkerServer::publish( visualization_msgs::InteractiveMarkerUpdate &update ) 00438 { 00439 update.server_id = server_id_; 00440 update.seq_num = seq_num_; 00441 update_pub_.publish( update ); 00442 } 00443 00444 00445 void InteractiveMarkerServer::doSetPose( M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header ) 00446 { 00447 if ( update_it == pending_updates_.end() ) 00448 { 00449 update_it = pending_updates_.insert( std::make_pair( name, UpdateContext() ) ).first; 00450 update_it->second.update_type = UpdateContext::POSE_UPDATE; 00451 } 00452 else if ( update_it->second.update_type != UpdateContext::FULL_UPDATE ) 00453 { 00454 update_it->second.update_type = UpdateContext::POSE_UPDATE; 00455 } 00456 00457 update_it->second.int_marker.pose = pose; 00458 update_it->second.int_marker.header = header; 00459 ROS_DEBUG( "Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z ); 00460 } 00461 00462 00463 }