interactive_marker_server.py
Go to the documentation of this file.
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 rospy
00033 
00034 from std_msgs.msg import Header
00035 
00036 from visualization_msgs.msg import InteractiveMarker
00037 from visualization_msgs.msg import InteractiveMarkerFeedback
00038 from visualization_msgs.msg import InteractiveMarkerInit
00039 from visualization_msgs.msg import InteractiveMarkerPose
00040 from visualization_msgs.msg import InteractiveMarkerUpdate
00041 
00042 from threading import Lock
00043 
00044 
00045 # Represents a single marker
00046 class MarkerContext:
00047 
00048     def __init__(self):
00049         self.last_feedback = rospy.Time.now()
00050         self.last_client_id = ""
00051         self.default_feedback_cb = None
00052         self.feedback_cbs = dict()
00053         self.int_marker = InteractiveMarker()
00054 
00055 
00056 # Represents an update to a single marker
00057 class UpdateContext:
00058     FULL_UPDATE = 0
00059     POSE_UPDATE = 1
00060     ERASE = 2
00061 
00062     def __init__(self):
00063         self.update_type = self.FULL_UPDATE
00064         self.int_marker = InteractiveMarker()
00065         self.default_feedback_cb = None
00066         self.feedback_cbs = dict()
00067 
00068 
00069 ## Acts as a server displaying a set of interactive markers.
00070 ##
00071 ## Note: Keep in mind that changes made by calling insert(), erase(), setCallback() etc.
00072 ##       are not applied until calling applyChanges().
00073 class InteractiveMarkerServer:
00074     DEFAULT_FEEDBACK_CB = 255
00075 
00076     ## @brief Create an InteractiveMarkerServer and associated ROS connections
00077     ## @param topic_ns      The interface will use the topics topic_ns/update and
00078     ##                      topic_ns/feedback for communication.
00079     ## @param server_id     If you run multiple servers on the same topic from
00080     ##                      within the same node, you will need to assign different names to them.
00081     ##                      Otherwise, leave this empty.
00082     def __init__(self, topic_ns, server_id="", q_size=100):
00083         self.topic_ns = topic_ns
00084         self.seq_num = 0
00085         self.mutex = Lock()
00086 
00087         self.server_id = rospy.get_name() + server_id
00088 
00089         # contains the current state of all markers
00090         # string : MarkerContext
00091         self.marker_contexts = dict()
00092 
00093         # updates that have to be sent on the next publish
00094         # string : UpdateContext
00095         self.pending_updates = dict()
00096 
00097         self.init_pub = rospy.Publisher(topic_ns+"/update_full", InteractiveMarkerInit, latch=True)
00098         self.update_pub = rospy.Publisher(topic_ns+"/update", InteractiveMarkerUpdate)
00099 
00100         rospy.Subscriber(topic_ns+"/feedback", InteractiveMarkerFeedback, self.processFeedback, queue_size=q_size)
00101         rospy.Timer(rospy.Duration(0.5), self.keepAlive)
00102 
00103         self.publishInit()
00104 
00105     ## @brief Destruction of the interface will lead to all managed markers being cleared.
00106     def __del__(self):
00107         self.clear()
00108         self.applyChanges()
00109 
00110     ## @brief Add or replace a marker.
00111     ## Note: Changes to the marker will not take effect until you call applyChanges().
00112     ## The callback changes immediately.
00113     ## @param marker The marker to be added or replaced
00114     ## @param feedback_cb Function to call on the arrival of a feedback message.
00115     ## @param feedback_type Type of feedback for which to call the feedback.
00116     def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB):
00117         with self.mutex:
00118             try:
00119                 update = self.pending_updates[marker.name]
00120             except:
00121                 update = UpdateContext()
00122                 self.pending_updates[marker.name] = update
00123             update.update_type = UpdateContext.FULL_UPDATE
00124             update.int_marker = marker
00125         if feedback_cb != -1:
00126             self.setCallback(marker.name, feedback_cb, feedback_type)
00127 
00128     ## @brief Update the pose of a marker with the specified name
00129     ## Note: This change will not take effect until you call applyChanges()
00130     ## @param name Name of the interactive marker
00131     ## @param pose The new pose
00132     ## @param header Header replacement. Leave this empty to use the previous one.
00133     ## @return True if a marker with that name exists
00134     def setPose(self, name, pose, header=Header()):
00135         with self.mutex:
00136             try:
00137                 marker_context = self.marker_contexts[name]
00138             except:
00139                 marker_context = None
00140             try:
00141                 update = self.pending_updates[name]
00142             except:
00143                 update = None
00144             # if there's no marker and no pending addition for it, we can't update the pose
00145             if marker_context is None and update is None:
00146                 return False
00147             if update is not None and update.update_type == UpdateContext.FULL_UPDATE:
00148                 return False
00149 
00150             if header.frame_id is None or header.frame_id == "":
00151                 # keep the old header
00152                 self.doSetPose(update, name, pose, marker_context.int_marker.header)
00153             else:
00154                 self.doSetPose(update, name, pose, header)
00155             return True
00156 
00157     ## @brief Erase the marker with the specified name
00158     ## Note: This change will not take effect until you call applyChanges().
00159     ## @param name Name of the interactive marker
00160     ## @return True if a marker with that name exists
00161     def erase(self, name):
00162         with self.mutex:
00163             try:
00164                 self.pending_updates[name].update_type = UpdateContext.ERASE
00165                 return True
00166             except:
00167                 try:
00168                     self.marker_contexts[name]  # check exists
00169                     update = UpdateContext()
00170                     update.update_type = UpdateContext.ERASE
00171                     self.pending_updates[name] = update
00172                     return True
00173                 except:
00174                     return False
00175 
00176     ## @brief Clear all markers.
00177     ## Note: This change will not take effect until you call applyChanges().
00178     def clear(self):
00179         self.pending_updates = dict()
00180         for marker_name in self.marker_contexts.keys():
00181             self.erase(marker_name)
00182 
00183     ## @brief Add or replace a callback function for the specified marker.
00184     ## Note: This change will not take effect until you call applyChanges().
00185     ## The server will try to call any type-specific callback first.
00186     ## If none is set, it will call the default callback.
00187     ## If a callback for the given type already exists, it will be replaced.
00188     ## To unset a type-specific callback, pass in an empty one.
00189     ## @param name           Name of the interactive marker
00190     ## @param feedback_cb    Function to call on the arrival of a feedback message.
00191     ## @param feedback_type  Type of feedback for which to call the feedback.
00192     ##                       Leave this empty to make this the default callback.
00193     def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB):
00194         with self.mutex:
00195             try:
00196                 marker_context = self.marker_contexts[name]
00197             except:
00198                 marker_context = None
00199             try:
00200                 update = self.pending_updates[name]
00201             except:
00202                 update = None
00203             if marker_context is None and update is None:
00204                 return False
00205 
00206             # we need to overwrite both the callbacks for the actual marker
00207             # and the update, if there's any
00208             if marker_context:
00209                 # the marker exists, so we can just overwrite the existing callbacks
00210                 if feedback_type == self.DEFAULT_FEEDBACK_CB:
00211                     marker_context.default_feedback_cb = feedback_cb
00212                 else:
00213                     if feedback_cb:
00214                         marker_context.feedback_cbs[feedback_type] = feedback_cb
00215                     else:
00216                         del marker_context.feedback_cbs[feedback_type]
00217             if update:
00218                 if feedback_type == self.DEFAULT_FEEDBACK_CB:
00219                     update.default_feedback_cb = feedback_cb
00220                 else:
00221                     if feedback_cb:
00222                         update.feedback_cbs[feedback_type] = feedback_cb
00223                     else:
00224                         del update.feedback_cbs[feedback_type]
00225             return True
00226 
00227     ## @brief Apply changes made since the last call to this method &
00228     ## broadcast an update to all clients.
00229     def applyChanges(self):
00230         with self.mutex:
00231             if len(self.pending_updates.keys()) == 0:
00232                 return
00233 
00234             update_msg = InteractiveMarkerUpdate()
00235             update_msg.type = InteractiveMarkerUpdate.UPDATE
00236 
00237             for name, update in self.pending_updates.items():
00238                 if update.update_type == UpdateContext.FULL_UPDATE:
00239                     try:
00240                         marker_context = self.marker_contexts[name]
00241                     except:
00242                         rospy.logdebug("Creating new context for " + name)
00243                         # create a new int_marker context
00244                         marker_context = MarkerContext()
00245                         marker_context.default_feedback_cb = update.default_feedback_cb
00246                         marker_context.feedback_cbs = update.feedback_cbs
00247                         self.marker_contexts[name] = marker_context
00248 
00249                     marker_context.int_marker = update.int_marker
00250                     update_msg.markers.append(marker_context.int_marker)
00251 
00252                 elif update.update_type == UpdateContext.POSE_UPDATE:
00253                     try:
00254                         marker_context = self.marker_contexts[name]
00255                         marker_context.int_marker.pose = update.int_marker.pose
00256                         marker_context.int_marker.header = update.int_marker.header
00257 
00258                         pose_update = InteractiveMarkerPose()
00259                         pose_update.header = marker_context.int_marker.header
00260                         pose_update.pose = marker_context.int_marker.pose
00261                         pose_update.name = marker_context.int_marker.name
00262                         update_msg.poses.append(pose_update)
00263                     except:
00264                         rospy.logerr("""\
00265 Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""")
00266 
00267                 elif update.update_type == UpdateContext.ERASE:
00268                     try:
00269                         marker_context = self.marker_contexts[name]
00270                         del self.marker_contexts[name]
00271                         update_msg.erases.append(name)
00272                     except:
00273                         pass
00274                 self.pending_updates = dict()
00275 
00276         self.seq_num += 1
00277         self.publish(update_msg)
00278         self.publishInit()
00279 
00280     ## @brief Get marker by name
00281     ## @param name Name of the interactive marker
00282     ## @return Marker if exists, None otherwise
00283     def get(self, name):
00284         try:
00285             update = self.pending_updates[name]
00286         except:
00287             try:
00288                 marker_context = self.marker_contexts[name]
00289             except:
00290                 return None
00291             return marker_context.int_marker
00292 
00293         # if there's an update pending, we'll have to account for that
00294         if update.update_type == UpdateContext.ERASE:
00295             return None
00296         elif update.update_type == UpdateContext.POSE_UPDATE:
00297             try:
00298                 marker_context = self.marker_contexts[name]
00299             except:
00300                 return None
00301             int_marker = marker_context.int_marker
00302             int_marker.pose = update.int_marker.pose
00303             return int_marker
00304         elif update.update_type == UpdateContext.FULL_UPDATE:
00305             return update.int_marker
00306         return None
00307 
00308     # update marker pose & call user callback.
00309     def processFeedback(self, feedback):
00310         with self.mutex:
00311             # ignore feedback for non-existing markers
00312             try:
00313                 marker_context = self.marker_contexts[feedback.marker_name]
00314             except:
00315                 return
00316 
00317             # if two callers try to modify the same marker, reject (timeout= 1 sec)
00318             if marker_context.last_client_id != feedback.client_id \
00319                and (rospy.Time.now() - marker_context.last_feedback).to_sec() < 1.0:
00320                 rospy.logdebug("Rejecting feedback for " +
00321                                feedback.marker_name +
00322                                ": conflicting feedback from separate clients.")
00323                 return
00324 
00325             marker_context.last_feedback = rospy.Time.now()
00326             marker_context.last_client_id = feedback.client_id
00327 
00328             if feedback.event_type == feedback.POSE_UPDATE:
00329                 if marker_context.int_marker.header.stamp == rospy.Time():
00330                     # keep the old header
00331                     try:
00332                         self.doSetPose(self.pending_updates[feedback.marker_name],
00333                                        feedback.marker_name,
00334                                        feedback.pose,
00335                                        marker_context.int_marker.header)
00336                     except:
00337                         self.doSetPose(None,
00338                                        feedback.marker_name,
00339                                        feedback.pose,
00340                                        marker_context.int_marker.header)
00341                 else:
00342                     try:
00343                         self.doSetPose(self.pending_updates[feedback.marker_name],
00344                                        feedback.marker_name,
00345                                        feedback.pose,
00346                                        feedback.header)
00347                     except:
00348                         self.doSetPose(None, feedback.marker_name, feedback.pose, feedback.header)
00349 
00350         # call feedback handler
00351         try:
00352             feedback_cb = marker_context.feedback_cbs[feedback.event_type]
00353             feedback_cb(feedback)
00354         except KeyError:
00355             #try:
00356             marker_context.default_feedback_cb(feedback)
00357             #except:
00358                 #pass
00359 
00360     # send an empty update to keep the client GUIs happy
00361     def keepAlive(self, msg):
00362         empty_msg = InteractiveMarkerUpdate()
00363         empty_msg.type = empty_msg.KEEP_ALIVE
00364         self.publish(empty_msg)
00365 
00366     # increase sequence number & publish an update
00367     def publish(self, update):
00368         update.server_id = self.server_id
00369         update.seq_num = self.seq_num
00370         self.update_pub.publish(update)
00371 
00372     # publish the current complete state to the latched "init" topic
00373     def publishInit(self):
00374         with self.mutex:
00375             init = InteractiveMarkerInit()
00376             init.server_id = self.server_id
00377             init.seq_num = self.seq_num
00378 
00379             for name, marker_context in self.marker_contexts.items():
00380                 rospy.logdebug("Publishing " + name)
00381                 init.markers.append(marker_context.int_marker)
00382 
00383             self.init_pub.publish(init)
00384 
00385     # update pose, schedule update without locking
00386     def doSetPose(self, update, name, pose, header):
00387         if update is None:
00388             update = UpdateContext()
00389             update.update_type = UpdateContext.POSE_UPDATE
00390             self.pending_updates[name] = update
00391         elif update.update_type != UpdateContext.FULL_UPDATE:
00392             update.update_type = UpdateContext.POSE_UPDATE
00393 
00394         update.int_marker.pose = pose
00395         update.int_marker.header = header
00396         rospy.logdebug("Marker '" + name + "' is now at " +
00397                        str(pose.position.x) + ", " + str(pose.position.y) +
00398                        ", " + str(pose.position.z))


interactive_markers
Author(s): David Gossow
autogenerated on Fri Aug 28 2015 11:11:26