If you want to create a node providing a set of interactive markers, you need to instantiate an InteractiveMarkerServer object. This will handle the connection to the client (usually RViz) and make sure that all changes you make are being transmitted and that your application is being notified of all the actions the user performs on the interactive markers.
# Note: Keep in mind that changes made by calling insert(), erase(), setCallback() etc. # are not applied until calling applyChanges(). classInteractiveMarkerServer: DEFAULT_FEEDBACK_CB = 255 ## @brief Create an InteractiveMarkerServer and associated ROS connections ## @param topic_ns The interface will use the topics topic_ns/update and ## topic_ns/feedback for communication. ## @param server_id If you run multiple servers on the same topic from ## within the same node, you will need to assign different names to them. ## Otherwise, leave this empty. def__init__(self, topic_ns, server_id="", q_size=100): self.topic_ns = topic_ns self.seq_num = 0 self.mutex = Lock() self.server_id = rospy.get_name() + server_id # 当前markers的当前状态 self.marker_contexts = dict() # 下一次publish需要发布的Markers，也是发生update的markers self.pending_updates = dict() self.init_pub = rospy.Publisher(topic_ns+"/update_full", InteractiveMarkerInit, latch=True, queue_size=100) # 初始的发布器，发布所有的InteractiveMarker self.update_pub = rospy.Publisher(topic_ns+"/update", InteractiveMarkerUpdate, queue_size=100) # 只发布发生改变的InteractiveMarker， 注意与init_pub发布消息类型不一样 rospy.Subscriber(topic_ns+"/feedback", InteractiveMarkerFeedback, self.processFeedback, queue_size=q_size) # 订阅器，订阅哪些marker发生了变化 rospy.Timer(rospy.Duration(0.5), self.keepAlive) # 保持node发布订阅从而一直alive self.publishInit() # 当前其实没有任何markers要发布，只是发布了个msg头 ## @brief 析构函数接口 will lead to all managed markers being cleared. def__del__(self): self.clear() self.applyChanges()
1 2 3 4 5 6 7 8 9 10 11
# send an empty update to keep the client GUIs happy defkeepAlive(self, msg): empty_msg = InteractiveMarkerUpdate() empty_msg.type = empty_msg.KEEP_ALIVE self.publish(empty_msg) # 这个就是发布一个空的UpdateMarker， 没有任何改变，只是保持连接 # increase sequence number & publish an update defpublish(self, update): update.server_id = self.server_id update.seq_num = self.seq_num self.update_pub.publish(update)
1 2 3 4 5 6 7 8 9 10 11 12
# update pose, schedule update without locking defpublishInit(self): with self.mutex: init = InteractiveMarkerInit() init.server_id = self.server_id init.seq_num = self.seq_num for name, marker_context in self.marker_contexts.items(): rospy.logdebug("Publishing " + name) init.markers.append(marker_context.int_marker) self.init_pub.publish(init) # 发布初始的markers
## @brief Add or replace a marker. ## Note: Changes to the marker will not take effect until you call applyChanges(). ## The callback changes immediately. ## @param marker The marker to be added or replaced ## @param feedback_cb Function to call on the arrival of a feedback message. ## @param feedback_type Type of feedback for which to call the feedback. definsert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB): #插入marker，其实也是一种update with self.mutex: try: # 首先看看要insert的marker是不是在等待update的dict中 update = self.pending_updates[marker.name] except:# 不在的话，就新建一个UpdateContext类型，内容是marker update = UpdateContext() self.pending_updates[marker.name] = update update.update_type = UpdateContext.FULL_UPDATE # 更新的类型，包含pose和颜色等 update.int_marker = marker if feedback_cb != -1: # 是否给该marker添加反馈回调函数，添加的话，当订阅器收到对应marker的feedback之后就会调用回调函数处理。 self.setCallback(marker.name, feedback_cb, feedback_type) defsetCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB): # 判断marker。name的对应marker，然后修改其回调函数 with self.mutex: try: marker_context = self.marker_contexts[name] except: marker_context = None try: update = self.pending_updates[name] except: update = None if marker_context isNoneand update isNone: returnFalse
# we need to overwrite both the callbacks for the actual marker # and the update, if there's any if marker_context: # the marker exists, so we can just overwrite the existing callbacks if feedback_type == self.DEFAULT_FEEDBACK_CB: marker_context.default_feedback_cb = feedback_cb else: if feedback_cb: # 可以定义多个回调函数 marker_context.feedback_cbs[feedback_type] = feedback_cb else: del marker_context.feedback_cbs[feedback_type] if update: if feedback_type == self.DEFAULT_FEEDBACK_CB: update.default_feedback_cb = feedback_cb else: if feedback_cb: update.feedback_cbs[feedback_type] = feedback_cb else: del update.feedback_cbs[feedback_type] returnTrue
# create a non-interactive control which contains the box box_control = InteractiveMarkerControl() box_control.always_visible = True box_control.markers.append( box_marker )
# add the control to the interactive marker int_marker.controls.append( box_control )
# create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows rotate_control = InteractiveMarkerControl() rotate_control.name = "move_x" rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# add the control to the interactive marker int_marker.controls.append(rotate_control);
# add the interactive marker to our collection & # tell the server to call processFeedback() when feedback arrives for it server.insert(int_marker, processFeedback) # 将创建的消息和回调函数注册到interactive marker server中
# 'commit' changes and send to all clients server.applyChanges() # 调用server中的函数 rospy.spin()
