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 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
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
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
00070
00071
00072
00073 class InteractiveMarkerServer:
00074 DEFAULT_FEEDBACK_CB = 255
00075
00076
00077
00078
00079
00080
00081
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
00090
00091 self.marker_contexts = dict()
00092
00093
00094
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
00106 def __del__(self):
00107 self.clear()
00108 self.applyChanges()
00109
00110
00111
00112
00113
00114
00115
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
00129
00130
00131
00132
00133
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
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
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
00158
00159
00160
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]
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
00177
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
00184
00185
00186
00187
00188
00189
00190
00191
00192
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
00207
00208 if marker_context:
00209
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
00228
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
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
00281
00282
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
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
00309 def processFeedback(self, feedback):
00310 with self.mutex:
00311
00312 try:
00313 marker_context = self.marker_contexts[feedback.marker_name]
00314 except:
00315 return
00316
00317
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
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
00351 try:
00352 feedback_cb = marker_context.feedback_cbs[feedback.event_type]
00353 feedback_cb(feedback)
00354 except KeyError:
00355
00356 marker_context.default_feedback_cb(feedback)
00357
00358
00359
00360
00361 def keepAlive(self, msg):
00362 empty_msg = InteractiveMarkerUpdate()
00363 empty_msg.type = empty_msg.KEEP_ALIVE
00364 self.publish(empty_msg)
00365
00366
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
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
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))