$search
00001 #!/usr/bin/env python 00002 00003 # Copyright (c) 2011, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions are met: 00008 # 00009 # * Redistributions of source code must retain the above copyright 00010 # notice, this list of conditions and the following disclaimer. 00011 # * Redistributions in binary form must reproduce the above copyright 00012 # notice, this list of conditions and the following disclaimer in the 00013 # documentation and/or other materials provided with the distribution. 00014 # * Neither the name of the Willow Garage, Inc. nor the names of its 00015 # contributors may be used to endorse or promote products derived from 00016 # this software without specific prior written permission. 00017 # 00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 # POSSIBILITY OF SUCH DAMAGE. 00029 00030 # Author: Michael Ferguson 00031 00032 import roslib; roslib.load_manifest("interactive_markers") 00033 import rospy 00034 00035 from visualization_msgs.msg import * 00036 from std_msgs.msg import Header 00037 from threading import Lock 00038 00039 00040 # Represents a single marker 00041 class MarkerContext: 00042 00043 def __init__(self): 00044 self.last_feedback = rospy.Time.now() 00045 self.last_client_id = "" 00046 self.default_feedback_cb = None 00047 self.feedback_cbs = dict() 00048 self.int_marker = InteractiveMarker() 00049 00050 00051 # Represents an update to a single marker 00052 class UpdateContext: 00053 FULL_UPDATE = 0 00054 POSE_UPDATE = 1 00055 ERASE = 2 00056 00057 def __init__(self): 00058 self.update_type = self.FULL_UPDATE 00059 self.int_marker = InteractiveMarker() 00060 self.default_feedback_cb = None 00061 self.feedback_cbs = dict() 00062 00063 00064 ## Acts as a server displaying a set of interactive markers. 00065 ## 00066 ## Note: Keep in mind that changes made by calling insert(), erase(), setCallback() etc. 00067 ## are not applied until calling applyChanges(). 00068 class InteractiveMarkerServer: 00069 DEFAULT_FEEDBACK_CB = 255 00070 00071 ## @brief Create an InteractiveMarkerServer and associated ROS connections 00072 ## @param topic_ns The interface will use the topics topic_ns/update and 00073 ## topic_ns/feedback for communication. 00074 ## @param server_id If you run multiple servers on the same topic from 00075 ## within the same node, you will need to assign different names to them. 00076 ## Otherwise, leave this empty. 00077 def __init__(self, topic_ns, server_id="", q_size=100): 00078 self.topic_ns = topic_ns 00079 self.seq_num = 0 00080 self.mutex = Lock() 00081 00082 self.server_id = rospy.get_name() + server_id 00083 00084 # contains the current state of all markers 00085 # string : MarkerContext 00086 self.marker_contexts = dict() 00087 00088 # updates that have to be sent on the next publish 00089 # string : UpdateContext 00090 self.pending_updates = dict() 00091 00092 self.init_pub = rospy.Publisher(topic_ns+"/update_full", InteractiveMarkerInit, latch=True) 00093 self.update_pub = rospy.Publisher(topic_ns+"/update", InteractiveMarkerUpdate) 00094 00095 rospy.Subscriber(topic_ns+"/feedback", InteractiveMarkerFeedback, self.processFeedback, queue_size=q_size) 00096 rospy.Timer(rospy.Duration(0.5), self.keepAlive) 00097 00098 self.publishInit() 00099 00100 ## @brief Destruction of the interface will lead to all managed markers being cleared. 00101 def __del__(self): 00102 self.clear() 00103 self.applyChanges() 00104 00105 ## @brief Add or replace a marker. 00106 ## Note: Changes to the marker will not take effect until you call applyChanges(). 00107 ## The callback changes immediately. 00108 ## @param marker The marker to be added or replaced 00109 ## @param feedback_cb Function to call on the arrival of a feedback message. 00110 ## @param feedback_type Type of feedback for which to call the feedback. 00111 def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB): 00112 with self.mutex: 00113 try: 00114 update = self.pending_updates[marker.name] 00115 except: 00116 update = UpdateContext() 00117 self.pending_updates[marker.name] = update 00118 00119 update.update_type = UpdateContext.FULL_UPDATE 00120 update.int_marker = marker 00121 if feedback_cb != -1: 00122 self.setCallback( marker.name, feedback_cb, feedback_type ) 00123 00124 ## @brief Update the pose of a marker with the specified name 00125 ## Note: This change will not take effect until you call applyChanges() 00126 ## @param name Name of the interactive marker 00127 ## @param pose The new pose 00128 ## @param header Header replacement. Leave this empty to use the previous one. 00129 ## @return True if a marker with that name exists 00130 def setPose(self, name, pose, header=Header()): 00131 with self.mutex: 00132 try: 00133 marker_context = self.marker_contexts[name] 00134 except: 00135 marker_context = None 00136 try: 00137 update = self.pending_updates[name] 00138 except: 00139 update = None 00140 # if there's no marker and no pending addition for it, we can't update the pose 00141 if marker_context == None and update == None: 00142 return False 00143 if update != None and update.update_type == UpdateContext.FULL_UPDATE: 00144 return False 00145 00146 if header.frame_id == None or header.frame_id == "": 00147 # keep the old header 00148 self.doSetPose( update, name, pose, marker_context.int_marker.header ) 00149 else: 00150 self.doSetPose( update, name, pose, header ) 00151 return True 00152 00153 ## @brief Erase the marker with the specified name 00154 ## Note: This change will not take effect until you call applyChanges(). 00155 ## @param name Name of the interactive marker 00156 ## @return True if a marker with that name exists 00157 def erase(self, name): 00158 with self.mutex: 00159 try: 00160 self.pending_updates[name].update_type = UpdateContext.ERASE 00161 return True 00162 except: 00163 try: 00164 marker_context = self.marker_contexts[name] # check exists 00165 update = UpdateContext() 00166 update.update_type = UpdateContext.ERASE 00167 self.pending_updates[name] = update 00168 return True 00169 except: 00170 return False 00171 00172 ## @brief Clear all markers. 00173 ## Note: This change will not take effect until you call applyChanges(). 00174 def clear(self): 00175 self.pending_updates = dict() 00176 self.marker_contexts = dict() 00177 00178 ## @brief Add or replace a callback function for the specified marker. 00179 ## Note: This change will not take effect until you call applyChanges(). 00180 ## The server will try to call any type-specific callback first. 00181 ## If none is set, it will call the default callback. 00182 ## If a callback for the given type already exists, it will be replaced. 00183 ## To unset a type-specific callback, pass in an empty one. 00184 ## @param name Name of the interactive marker 00185 ## @param feedback_cb Function to call on the arrival of a feedback message. 00186 ## @param feedback_type Type of feedback for which to call the feedback. 00187 ## Leave this empty to make this the default callback. 00188 def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB): 00189 with self.mutex: 00190 try: 00191 marker_context = self.marker_contexts[name] 00192 except: 00193 marker_context = None 00194 try: 00195 update = self.pending_updates[name] 00196 except: 00197 update = None 00198 if marker_context == None and update == None: 00199 return False 00200 00201 # we need to overwrite both the callbacks for the actual marker 00202 # and the update, if there's any 00203 if marker_context: 00204 # the marker exists, so we can just overwrite the existing callbacks 00205 if feedback_type == self.DEFAULT_FEEDBACK_CB: 00206 marker_context.default_feedback_cb = feedback_cb 00207 else: 00208 if feedback_cb: 00209 marker_context.feedback_cbs[feedback_type] = feedback_cb 00210 else: 00211 del marker_context.feedback_cbs[feedback_type] 00212 if update: 00213 if feedback_type == self.DEFAULT_FEEDBACK_CB: 00214 update.default_feedback_cb = feedback_cb 00215 else: 00216 if feedback_cb: 00217 update.feedback_cbs[feedback_type] = feedback_cb 00218 else: 00219 del update.feedback_cbs[feedback_type] 00220 return True 00221 00222 ## @brief Apply changes made since the last call to this method & 00223 ## broadcast an update to all clients. 00224 def applyChanges(self): 00225 with self.mutex: 00226 if len( self.pending_updates.keys() ) == 0: 00227 return 00228 00229 update_msg = InteractiveMarkerUpdate() 00230 update_msg.type = InteractiveMarkerUpdate.UPDATE 00231 00232 for name, update in self.pending_updates.items(): 00233 if update.update_type == UpdateContext.FULL_UPDATE: 00234 try: 00235 marker_context = self.marker_contexts[name] 00236 except: 00237 rospy.logdebug("Creating new context for " + name); 00238 # create a new int_marker context 00239 marker_context = MarkerContext() 00240 marker_context.default_feedback_cb = update.default_feedback_cb 00241 marker_context.feedback_cbs = update.feedback_cbs 00242 self.marker_contexts[name] = marker_context 00243 00244 marker_context.int_marker = update.int_marker 00245 update_msg.markers.append( marker_context.int_marker ) 00246 00247 elif update.update_type == UpdateContext.POSE_UPDATE: 00248 try: 00249 marker_context = self.marker_contexts[name] 00250 marker_context.int_marker.pose = update.int_marker.pose 00251 00252 pose_update = InteractiveMarkerPose() 00253 pose_update.header = marker_context.int_marker.header; 00254 pose_update.pose = marker_context.int_marker.pose; 00255 pose_update.name = marker_context.int_marker.name; 00256 update_msg.poses.append( pose_update ) 00257 except: 00258 rospy.logerr("Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.") 00259 00260 elif update.update_type == UpdateContext.ERASE: 00261 try: 00262 marker_context = self.marker_contexts[name] 00263 del self.marker_contexts[name] 00264 update_msg.erases.append( name ) 00265 except: 00266 pass 00267 self.pending_updates = dict() 00268 00269 self.seq_num += 1 00270 self.publish( update_msg ) 00271 self.publishInit() 00272 00273 ## @brief Get marker by name 00274 ## @param name Name of the interactive marker 00275 ## @return Marker if exists, None otherwise 00276 def get(self, name): 00277 try: 00278 update = self.pending_updates[name] 00279 except: 00280 try: 00281 marker_context = self.marker_contexts[name] 00282 except: 00283 return None 00284 return marker_context.int_marker 00285 00286 # if there's an update pending, we'll have to account for that 00287 if update.update_type == UpdateContext.ERASE: 00288 return None 00289 elif update.update_type == UpdateContext.POSE_UPDATE: 00290 try: 00291 marker_context = self.marker_contexts[name] 00292 except: 00293 return None 00294 int_marker = marker_context.int_marker 00295 int_marker.pose = update.int_marker.pose 00296 return int_marker 00297 elif update.update_type == UpdateContext.FULL_UPDATE: 00298 return update.int_marker 00299 return None 00300 00301 # update marker pose & call user callback. 00302 def processFeedback(self, feedback): 00303 with self.mutex: 00304 # ignore feedback for non-existing markers 00305 try: 00306 marker_context = self.marker_contexts[feedback.marker_name] 00307 except: 00308 return 00309 00310 # if two callers try to modify the same marker, reject (timeout= 1 sec) 00311 if marker_context.last_client_id != feedback.client_id and (rospy.Time.now() - marker_context.last_feedback).to_sec() < 1.0: 00312 rospy.logdebug("Rejecting feedback for " + feedback.marker_name + ": conflicting feedback from separate clients.") 00313 return 00314 00315 marker_context.last_feedback = rospy.Time.now() 00316 marker_context.last_client_id = feedback.client_id 00317 00318 if feedback.event_type == feedback.POSE_UPDATE: 00319 if marker_context.int_marker.header.stamp == rospy.Time(): 00320 # keep the old header 00321 try: 00322 self.doSetPose( self.pending_updates[feedback.marker_name], feedback.marker_name, feedback.pose, marker_context.int_marker.header ) 00323 except: 00324 self.doSetPose( None, feedback.marker_name, feedback.pose, marker_context.int_marker.header ) 00325 else: 00326 try: 00327 self.doSetPose( self.pending_updates[feedback.marker_name], feedback.marker_name, feedback.pose, feedback.header ) 00328 except: 00329 self.doSetPose( None, feedback.marker_name, feedback.pose, feedback.header ) 00330 00331 # call feedback handler 00332 try: 00333 feedback_cb = marker_context.feedback_cbs[feedback.event_type] 00334 feedback_cb(feedback) 00335 except KeyError: 00336 #try: 00337 marker_context.default_feedback_cb( feedback ) 00338 #except: 00339 #pass 00340 00341 # send an empty update to keep the client GUIs happy 00342 def keepAlive(self, msg): 00343 empty_msg = InteractiveMarkerUpdate() 00344 empty_msg.type = empty_msg.KEEP_ALIVE 00345 self.publish( empty_msg ) 00346 00347 # increase sequence number & publish an update 00348 def publish(self, update): 00349 update.server_id = self.server_id 00350 update.seq_num = self.seq_num 00351 self.update_pub.publish( update) 00352 00353 # publish the current complete state to the latched "init" topic 00354 def publishInit(self): 00355 with self.mutex: 00356 init = InteractiveMarkerInit() 00357 init.server_id = self.server_id 00358 init.seq_num = self.seq_num 00359 00360 for name, marker_context in self.marker_contexts.items(): 00361 rospy.logdebug("Publishing " + name ) 00362 init.markers.append( marker_context.int_marker ) 00363 00364 self.init_pub.publish( init ) 00365 00366 # update pose, schedule update without locking 00367 def doSetPose(self, update, name, pose, header): 00368 if update == None: 00369 update = UpdateContext() 00370 update.update_type = UpdateContext.POSE_UPDATE 00371 self.pending_updates[name] = update 00372 elif update.update_type != UpdateContext.FULL_UPDATE: 00373 update.update_type = UpdateContext.POSE_UPDATE 00374 00375 update.int_marker.pose = pose 00376 update.int_marker.header = header 00377 rospy.logdebug("Marker '" + name + "' is now at " + str(pose.position.x) + ", " + str(pose.position.y) + ", " + str(pose.position.z) ) 00378