34 from std_msgs.msg
import Header
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
42 from threading
import Lock
74 DEFAULT_FEEDBACK_CB = 255
82 def __init__(self, topic_ns, server_id="", q_size=100):
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)
100 rospy.Subscriber(topic_ns+
"/feedback", InteractiveMarkerFeedback, self.
processFeedback, queue_size=q_size)
101 rospy.Timer(rospy.Duration(0.5), self.
keepAlive, reset=
True)
116 def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB):
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)
134 def setPose(self, name, pose, header=Header()):
139 marker_context =
None 145 if marker_context
is None and update
is None:
147 if update
is not None and update.update_type == UpdateContext.FULL_UPDATE:
150 if header.frame_id
is None or header.frame_id ==
"":
152 self.
doSetPose(update, name, pose, marker_context.int_marker.header)
154 self.
doSetPose(update, name, pose, header)
170 update.update_type = UpdateContext.ERASE
180 for marker_name
in self.marker_contexts.keys():
181 self.
erase(marker_name)
193 def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB):
198 marker_context =
None 203 if marker_context
is None and update
is None:
211 marker_context.default_feedback_cb = feedback_cb
214 marker_context.feedback_cbs[feedback_type] = feedback_cb
216 del marker_context.feedback_cbs[feedback_type]
219 update.default_feedback_cb = feedback_cb
222 update.feedback_cbs[feedback_type] = feedback_cb
224 del update.feedback_cbs[feedback_type]
231 if len(self.pending_updates.keys()) == 0:
234 update_msg = InteractiveMarkerUpdate()
235 update_msg.type = InteractiveMarkerUpdate.UPDATE
237 for name, update
in self.pending_updates.items():
238 if update.update_type == UpdateContext.FULL_UPDATE:
242 rospy.logdebug(
"Creating new context for " + name)
245 marker_context.default_feedback_cb = update.default_feedback_cb
246 marker_context.feedback_cbs = update.feedback_cbs
249 marker_context.int_marker = update.int_marker
250 update_msg.markers.append(marker_context.int_marker)
252 elif update.update_type == UpdateContext.POSE_UPDATE:
255 marker_context.int_marker.pose = update.int_marker.pose
256 marker_context.int_marker.header = update.int_marker.header
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)
265 Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""")
267 elif update.update_type == UpdateContext.ERASE:
271 update_msg.erases.append(name)
291 return marker_context.int_marker
294 if update.update_type == UpdateContext.ERASE:
296 elif update.update_type == UpdateContext.POSE_UPDATE:
301 int_marker = marker_context.int_marker
302 int_marker.pose = update.int_marker.pose
304 elif update.update_type == UpdateContext.FULL_UPDATE:
305 return update.int_marker
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.")
325 marker_context.last_feedback = rospy.Time.now()
326 marker_context.last_client_id = feedback.client_id
328 if feedback.event_type == feedback.POSE_UPDATE:
329 if marker_context.int_marker.header.stamp == rospy.Time():
333 feedback.marker_name,
335 marker_context.int_marker.header)
338 feedback.marker_name,
340 marker_context.int_marker.header)
344 feedback.marker_name,
348 self.
doSetPose(
None, feedback.marker_name, feedback.pose, feedback.header)
352 feedback_cb = marker_context.feedback_cbs[feedback.event_type]
353 feedback_cb(feedback)
356 marker_context.default_feedback_cb(feedback)
362 empty_msg = InteractiveMarkerUpdate()
363 empty_msg.type = empty_msg.KEEP_ALIVE
370 self.update_pub.publish(update)
375 init = InteractiveMarkerInit()
379 for name, marker_context
in self.marker_contexts.items():
380 rospy.logdebug(
"Publishing " + name)
381 init.markers.append(marker_context.int_marker)
383 self.init_pub.publish(init)
389 update.update_type = UpdateContext.POSE_UPDATE
391 elif update.update_type != UpdateContext.FULL_UPDATE:
392 update.update_type = UpdateContext.POSE_UPDATE
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.
def clear(self)
Clear all markers.
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 publish(self, update)
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 doSetPose(self, update, name, pose, header)
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...
def processFeedback(self, feedback)
def get(self, name)
Get marker by name.