Go to the documentation of this file.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 visualization_msgs.msg import InteractiveMarkerFeedback
00035 from visualization_msgs.msg import MenuEntry
00036
00037
00038 class EntryContext:
00039 def __init__(self):
00040 self.title = ""
00041 self.command = ""
00042 self.command_type = 0
00043 self.sub_entries = list()
00044 self.visible = True
00045 self.check_state = 0
00046 self.feedback_cb = None
00047
00048
00049
00050
00051 class MenuHandler:
00052 NO_CHECKBOX = 0
00053 CHECKED = 1
00054 UNCHECKED = 2
00055
00056 def __init__(self):
00057 self.top_level_handles_ = list()
00058 self.entry_contexts_ = dict()
00059 self.current_handle_ = 1
00060 self.managed_markers_ = set()
00061
00062
00063 def insert(self, title, parent=None, command_type=MenuEntry.FEEDBACK, command="", callback=None):
00064 handle = self.doInsert(title, command_type, command, callback)
00065 if parent is not None:
00066 try:
00067 parent_context = self.entry_contexts_[parent]
00068 parent_context.sub_entries.append(handle)
00069 except:
00070 rospy.logerr("Parent menu entry " + str(parent) + " not found.")
00071 return None
00072 else:
00073 self.top_level_handles_.append(handle)
00074 return handle
00075
00076
00077 def setVisible(self, handle, visible):
00078 try:
00079 context = self.entry_contexts_[handle]
00080 context.visible = visible
00081 return True
00082 except:
00083 return False
00084
00085
00086 def setCheckState(self, handle, check_state):
00087 try:
00088 context = self.entry_contexts_[handle]
00089 context.check_state = check_state
00090 return True
00091 except:
00092 return False
00093
00094
00095
00096 def getCheckState(self, handle):
00097 try:
00098 context = self.entry_contexts_[handle]
00099 return context.check_state
00100 except:
00101 return None
00102
00103
00104
00105 def apply(self, server, marker_name):
00106 marker = server.get(marker_name)
00107 if not marker:
00108 self.managed_markers_.remove(marker_name)
00109 return False
00110
00111 marker.menu_entries = list()
00112 self.pushMenuEntries(self.top_level_handles_, marker.menu_entries, 0)
00113
00114 server.insert(marker, self.processFeedback, InteractiveMarkerFeedback.MENU_SELECT)
00115 self.managed_markers_.add(marker_name)
00116 return True
00117
00118
00119 def reApply(self, server):
00120 success = True
00121
00122
00123
00124 managed_markers = list(self.managed_markers_)
00125 for marker in managed_markers:
00126 success = self.apply(server, marker) and success
00127 return success
00128
00129
00130
00131 def getTitle(self, handle):
00132 try:
00133 return self.entry_contexts_[handle].title
00134 except:
00135 return None
00136
00137
00138 def processFeedback(self, feedback):
00139 try:
00140 context = self.entry_contexts_[feedback.menu_entry_id]
00141 context.feedback_cb(feedback)
00142 except KeyError:
00143 pass
00144
00145
00146
00147 def pushMenuEntries(self, handles_in, entries_out, parent_handle):
00148 for handle in handles_in:
00149 try:
00150 context = self.entry_contexts_[handle]
00151 if not context.visible:
00152 continue
00153 entries_out.append(self.makeEntry(context, handle, parent_handle))
00154 if not self.pushMenuEntries(context.sub_entries, entries_out, handle):
00155 return False
00156 except:
00157 rospy.logerr("Internal error: context handle not found! This is a bug in MenuHandler.")
00158 return False
00159 return True
00160
00161 def makeEntry(self, context, handle, parent_handle):
00162 menu_entry = MenuEntry()
00163 if context.check_state == self.NO_CHECKBOX:
00164 menu_entry.title = context.title
00165 elif context.check_state == self.CHECKED:
00166 menu_entry.title = "[x] "+context.title
00167 elif context.check_state == self.UNCHECKED:
00168 menu_entry.title = "[ ] "+context.title
00169
00170 menu_entry.command = context.command
00171 menu_entry.command_type = context.command_type
00172 menu_entry.id = handle
00173 menu_entry.parent_id = parent_handle
00174
00175 return menu_entry
00176
00177
00178 def doInsert(self, title, command_type, command, feedback_cb):
00179 handle = self.current_handle_
00180 self.current_handle_ += 1
00181
00182 context = EntryContext()
00183 context.title = title
00184 context.command = command
00185 context.command_type = command_type
00186 context.visible = True
00187 context.check_state = self.NO_CHECKBOX
00188 context.feedback_cb = feedback_cb
00189
00190 self.entry_contexts_[handle] = context
00191 return handle