interactive_marker_server.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2011, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright
12 # notice, this list of conditions and the following disclaimer in the
13 # documentation and/or other materials provided with the distribution.
14 # * Neither the name of the Willow Garage, Inc. nor the names of its
15 # contributors may be used to endorse or promote products derived from
16 # this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 # POSSIBILITY OF SUCH DAMAGE.
29 
30 # Author: Michael Ferguson
31 
32 import rospy
33 
34 from std_msgs.msg import Header
35 
36 from visualization_msgs.msg import InteractiveMarker
37 from visualization_msgs.msg import InteractiveMarkerFeedback
38 from visualization_msgs.msg import InteractiveMarkerInit
39 from visualization_msgs.msg import InteractiveMarkerPose
40 from visualization_msgs.msg import InteractiveMarkerUpdate
41 
42 from threading import Lock
43 
44 
45 # Represents a single marker
47 
48  def __init__(self):
49  self.last_feedback = rospy.Time.now()
50  self.last_client_id = ""
51  self.default_feedback_cb = None
52  self.feedback_cbs = dict()
53  self.int_marker = InteractiveMarker()
54 
55 
56 # Represents an update to a single marker
58  FULL_UPDATE = 0
59  POSE_UPDATE = 1
60  ERASE = 2
61 
62  def __init__(self):
64  self.int_marker = InteractiveMarker()
65  self.default_feedback_cb = None
66  self.feedback_cbs = dict()
67 
68 
69 ## Acts as a server displaying a set of interactive markers.
70 
74  DEFAULT_FEEDBACK_CB = 255
75 
76  ## @brief Create an InteractiveMarkerServer and associated ROS connections
77  ## @param topic_ns The interface will use the topics topic_ns/update and
78  ## topic_ns/feedback for communication.
79  ## @param server_id If you run multiple servers on the same topic from
80  ## within the same node, you will need to assign different names to them.
81  ## Otherwise, leave this empty.
82  def __init__(self, topic_ns, server_id="", q_size=100):
83  self.topic_ns = topic_ns
84  self.seq_num = 0
85  self.mutex = Lock()
86 
87  self.server_id = rospy.get_name() + server_id
88 
89  # contains the current state of all markers
90  # string : MarkerContext
91  self.marker_contexts = dict()
92 
93  # updates that have to be sent on the next publish
94  # string : UpdateContext
95  self.pending_updates = dict()
96 
97  self.init_pub = rospy.Publisher(topic_ns+"/update_full", InteractiveMarkerInit, latch=True, queue_size=100)
98  self.update_pub = rospy.Publisher(topic_ns+"/update", InteractiveMarkerUpdate, queue_size=100)
99 
100  rospy.Subscriber(topic_ns+"/feedback", InteractiveMarkerFeedback, self.processFeedback, queue_size=q_size)
101  rospy.Timer(rospy.Duration(0.5), self.keepAlive, reset=True)
102 
103  self.publishInit()
104 
105  ## @brief Destruction of the interface will lead to all managed markers being cleared.
106  def __del__(self):
107  self.clear()
108  self.applyChanges()
109 
110  ## @brief Add or replace a marker.
111  ## Note: Changes to the marker will not take effect until you call applyChanges().
112  ## The callback changes immediately.
113  ## @param marker The marker to be added or replaced
114  ## @param feedback_cb Function to call on the arrival of a feedback message.
115  ## @param feedback_type Type of feedback for which to call the feedback.
116  def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB):
117  with self.mutex:
118  try:
119  update = self.pending_updates[marker.name]
120  except:
121  update = UpdateContext()
122  self.pending_updates[marker.name] = update
123  update.update_type = UpdateContext.FULL_UPDATE
124  update.int_marker = marker
125  if feedback_cb != -1:
126  self.setCallback(marker.name, feedback_cb, feedback_type)
127 
128  ## @brief Update the pose of a marker with the specified name
129  ## Note: This change will not take effect until you call applyChanges()
130  ## @param name Name of the interactive marker
131  ## @param pose The new pose
132  ## @param header Header replacement. Leave this empty to use the previous one.
133  ## @return True if a marker with that name exists
134  def setPose(self, name, pose, header=Header()):
135  with self.mutex:
136  try:
137  marker_context = self.marker_contexts[name]
138  except:
139  marker_context = None
140  try:
141  update = self.pending_updates[name]
142  except:
143  update = None
144  # if there's no marker and no pending addition for it, we can't update the pose
145  if marker_context is None and update is None:
146  return False
147  if update is not None and update.update_type == UpdateContext.FULL_UPDATE:
148  return False
149 
150  if header.frame_id is None or header.frame_id == "":
151  # keep the old header
152  self.doSetPose(update, name, pose, marker_context.int_marker.header)
153  else:
154  self.doSetPose(update, name, pose, header)
155  return True
156 
157  ## @brief Erase the marker with the specified name
158  ## Note: This change will not take effect until you call applyChanges().
159  ## @param name Name of the interactive marker
160  ## @return True if a marker with that name exists
161  def erase(self, name):
162  with self.mutex:
163  try:
164  self.pending_updates[name].update_type = UpdateContext.ERASE
165  return True
166  except:
167  try:
168  self.marker_contexts[name] # check exists
169  update = UpdateContext()
170  update.update_type = UpdateContext.ERASE
171  self.pending_updates[name] = update
172  return True
173  except:
174  return False
175 
176  ## @brief Clear all markers.
177  ## Note: This change will not take effect until you call applyChanges().
178  def clear(self):
179  self.pending_updates = dict()
180  for marker_name in self.marker_contexts.keys():
181  self.erase(marker_name)
182 
183  ## @brief Add or replace a callback function for the specified marker.
184  ## Note: This change will not take effect until you call applyChanges().
185  ## The server will try to call any type-specific callback first.
186  ## If none is set, it will call the default callback.
187  ## If a callback for the given type already exists, it will be replaced.
188  ## To unset a type-specific callback, pass in an empty one.
189  ## @param name Name of the interactive marker
190  ## @param feedback_cb Function to call on the arrival of a feedback message.
191  ## @param feedback_type Type of feedback for which to call the feedback.
192  ## Leave this empty to make this the default callback.
193  def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB):
194  with self.mutex:
195  try:
196  marker_context = self.marker_contexts[name]
197  except:
198  marker_context = None
199  try:
200  update = self.pending_updates[name]
201  except:
202  update = None
203  if marker_context is None and update is None:
204  return False
205 
206  # we need to overwrite both the callbacks for the actual marker
207  # and the update, if there's any
208  if marker_context:
209  # the marker exists, so we can just overwrite the existing callbacks
210  if feedback_type == self.DEFAULT_FEEDBACK_CB:
211  marker_context.default_feedback_cb = feedback_cb
212  else:
213  if feedback_cb:
214  marker_context.feedback_cbs[feedback_type] = feedback_cb
215  else:
216  del marker_context.feedback_cbs[feedback_type]
217  if update:
218  if feedback_type == self.DEFAULT_FEEDBACK_CB:
219  update.default_feedback_cb = feedback_cb
220  else:
221  if feedback_cb:
222  update.feedback_cbs[feedback_type] = feedback_cb
223  else:
224  del update.feedback_cbs[feedback_type]
225  return True
226 
227  ## @brief Apply changes made since the last call to this method &
228  ## broadcast an update to all clients.
229  def applyChanges(self):
230  with self.mutex:
231  if len(self.pending_updates.keys()) == 0:
232  return
233 
234  update_msg = InteractiveMarkerUpdate()
235  update_msg.type = InteractiveMarkerUpdate.UPDATE
236 
237  for name, update in self.pending_updates.items():
238  if update.update_type == UpdateContext.FULL_UPDATE:
239  try:
240  marker_context = self.marker_contexts[name]
241  except:
242  rospy.logdebug("Creating new context for " + name)
243  # create a new int_marker context
244  marker_context = MarkerContext()
245  marker_context.default_feedback_cb = update.default_feedback_cb
246  marker_context.feedback_cbs = update.feedback_cbs
247  self.marker_contexts[name] = marker_context
248 
249  marker_context.int_marker = update.int_marker
250  update_msg.markers.append(marker_context.int_marker)
251 
252  elif update.update_type == UpdateContext.POSE_UPDATE:
253  try:
254  marker_context = self.marker_contexts[name]
255  marker_context.int_marker.pose = update.int_marker.pose
256  marker_context.int_marker.header = update.int_marker.header
257 
258  pose_update = InteractiveMarkerPose()
259  pose_update.header = marker_context.int_marker.header
260  pose_update.pose = marker_context.int_marker.pose
261  pose_update.name = marker_context.int_marker.name
262  update_msg.poses.append(pose_update)
263  except:
264  rospy.logerr("""\
265 Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""")
266 
267  elif update.update_type == UpdateContext.ERASE:
268  try:
269  marker_context = self.marker_contexts[name]
270  del self.marker_contexts[name]
271  update_msg.erases.append(name)
272  except:
273  pass
274  self.pending_updates = dict()
275 
276  self.seq_num += 1
277  self.publish(update_msg)
278  self.publishInit()
279 
280  ## @brief Get marker by name
281  ## @param name Name of the interactive marker
282  ## @return Marker if exists, None otherwise
283  def get(self, name):
284  try:
285  update = self.pending_updates[name]
286  except:
287  try:
288  marker_context = self.marker_contexts[name]
289  except:
290  return None
291  return marker_context.int_marker
292 
293  # if there's an update pending, we'll have to account for that
294  if update.update_type == UpdateContext.ERASE:
295  return None
296  elif update.update_type == UpdateContext.POSE_UPDATE:
297  try:
298  marker_context = self.marker_contexts[name]
299  except:
300  return None
301  int_marker = marker_context.int_marker
302  int_marker.pose = update.int_marker.pose
303  return int_marker
304  elif update.update_type == UpdateContext.FULL_UPDATE:
305  return update.int_marker
306  return None
307 
308  # update marker pose & call user callback.
309  def processFeedback(self, feedback):
310  with self.mutex:
311  # ignore feedback for non-existing markers
312  try:
313  marker_context = self.marker_contexts[feedback.marker_name]
314  except:
315  return
316 
317  # if two callers try to modify the same marker, reject (timeout= 1 sec)
318  if marker_context.last_client_id != feedback.client_id \
319  and (rospy.Time.now() - marker_context.last_feedback).to_sec() < 1.0:
320  rospy.logdebug("Rejecting feedback for " +
321  feedback.marker_name +
322  ": conflicting feedback from separate clients.")
323  return
324 
325  marker_context.last_feedback = rospy.Time.now()
326  marker_context.last_client_id = feedback.client_id
327 
328  if feedback.event_type == feedback.POSE_UPDATE:
329  if marker_context.int_marker.header.stamp == rospy.Time():
330  # keep the old header
331  try:
332  self.doSetPose(self.pending_updates[feedback.marker_name],
333  feedback.marker_name,
334  feedback.pose,
335  marker_context.int_marker.header)
336  except:
337  self.doSetPose(None,
338  feedback.marker_name,
339  feedback.pose,
340  marker_context.int_marker.header)
341  else:
342  try:
343  self.doSetPose(self.pending_updates[feedback.marker_name],
344  feedback.marker_name,
345  feedback.pose,
346  feedback.header)
347  except:
348  self.doSetPose(None, feedback.marker_name, feedback.pose, feedback.header)
349 
350  # call feedback handler
351  try:
352  feedback_cb = marker_context.feedback_cbs[feedback.event_type]
353  feedback_cb(feedback)
354  except KeyError:
355  #try:
356  marker_context.default_feedback_cb(feedback)
357  #except:
358  #pass
359 
360  # send an empty update to keep the client GUIs happy
361  def keepAlive(self, msg):
362  empty_msg = InteractiveMarkerUpdate()
363  empty_msg.type = empty_msg.KEEP_ALIVE
364  self.publish(empty_msg)
365 
366  # increase sequence number & publish an update
367  def publish(self, update):
368  update.server_id = self.server_id
369  update.seq_num = self.seq_num
370  self.update_pub.publish(update)
371 
372  # publish the current complete state to the latched "init" topic
373  def publishInit(self):
374  with self.mutex:
375  init = InteractiveMarkerInit()
376  init.server_id = self.server_id
377  init.seq_num = self.seq_num
378 
379  for name, marker_context in self.marker_contexts.items():
380  rospy.logdebug("Publishing " + name)
381  init.markers.append(marker_context.int_marker)
382 
383  self.init_pub.publish(init)
384 
385  # update pose, schedule update without locking
386  def doSetPose(self, update, name, pose, header):
387  if update is None:
388  update = UpdateContext()
389  update.update_type = UpdateContext.POSE_UPDATE
390  self.pending_updates[name] = update
391  elif update.update_type != UpdateContext.FULL_UPDATE:
392  update.update_type = UpdateContext.POSE_UPDATE
393 
394  update.int_marker.pose = pose
395  update.int_marker.header = header
396  rospy.logdebug("Marker '" + name + "' is now at " +
397  str(pose.position.x) + ", " + str(pose.position.y) +
398  ", " + str(pose.position.z))
def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB)
Add or replace a marker.
Acts as a server displaying a set of interactive markers.
def __init__(self, topic_ns, server_id="", q_size=100)
Create an InteractiveMarkerServer and associated ROS connections.
def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB)
Add or replace a callback function for the specified marker.
def __del__(self)
Destruction of the interface will lead to all managed markers being cleared.
def applyChanges(self)
Apply changes made since the last call to this method & broadcast an update to all clients...
def setPose(self, name, pose, header=Header())
Update the pose of a marker with the specified name Note: This change will not take effect until you ...
def erase(self, name)
Erase the marker with the specified name Note: This change will not take effect until you call applyC...


interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Thu Oct 8 2020 04:02:35