def __init__(self):
wx.Frame.__init__(self, None, -1, "Smach Viewer", size=(720, 480))
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# This is triggered every time the display needs to be updated
self._update_cond = threading.Condition()
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._containers = {}
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Populate the frame with widgets
self._populate_frame()
# Register mouse event callback for xdot widget
self.widget.register_select_callback(self.select_cb)
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._needs_refresh = True
# Start a thread in the background to update the server list
self._keep_running = True
self._server_list_thread = threading.Thread(
target=self._update_server_list)
self._server_list_thread.start()
# Start a thread in the background to update the graph display
self._update_graph_thread = threading.Thread(target=self._update_graph)
self._update_graph_thread.start()
# Start a thread in the background to update the tree display
self._update_tree_thread = threading.Thread(target=self._update_tree)
self._update_tree_thread.start()
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_smach.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('SmachPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(
self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(
self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
def __init__(self):
wx.Frame.__init__(self, None, -1, "Smach Viewer", size=(720,480))
# Create graph
self._containers = {}
self._top_containers = {}
self._update_cond = threading.Condition()
self._needs_refresh = True
self.dotstr = ''
vbox = wx.BoxSizer(wx.VERTICAL)
# Create Splitter
self.content_splitter = wx.SplitterWindow(self, -1,style = wx.SP_LIVE_UPDATE)
self.content_splitter.SetMinimumPaneSize(24)
self.content_splitter.SetSashGravity(0.85)
# Create viewer pane
viewer = wx.Panel(self.content_splitter,-1)
# Create smach viewer
nb = wx.Notebook(viewer,-1,style=wx.NB_TOP | wx.WANTS_CHARS)
viewer_box = wx.BoxSizer()
viewer_box.Add(nb,1,wx.EXPAND | wx.ALL, 4)
viewer.SetSizer(viewer_box)
# Create graph view
graph_view = wx.Panel(nb,-1)
gv_vbox = wx.BoxSizer(wx.VERTICAL)
graph_view.SetSizer(gv_vbox)
# Construct toolbar
toolbar = wx.ToolBar(graph_view, -1)
toolbar.AddControl(wx.StaticText(toolbar,-1,"Path: "))
# Path list
self.path_combo = wx.ComboBox(toolbar, -1, style=wx.CB_DROPDOWN)
self.path_combo .Bind(wx.EVT_COMBOBOX, self.set_path)
self.path_combo.Append('/')
self.path_combo.SetValue('/')
toolbar.AddControl(self.path_combo)
# Depth spinner
self.depth_spinner = wx.SpinCtrl(toolbar, -1,
size=wx.Size(50,-1),
min=-1,
max=1337,
initial=-1)
self.depth_spinner.Bind(wx.EVT_SPINCTRL,self.set_depth)
self._max_depth = -1
toolbar.AddControl(wx.StaticText(toolbar,-1," Depth: "))
toolbar.AddControl(self.depth_spinner)
# Label width spinner
self.width_spinner = wx.SpinCtrl(toolbar, -1,
size=wx.Size(50,-1),
min=1,
max=1337,
initial=40)
self.width_spinner.Bind(wx.EVT_SPINCTRL,self.set_label_width)
self._label_wrapper = textwrap.TextWrapper(40,break_long_words=True)
toolbar.AddControl(wx.StaticText(toolbar,-1," Label Width: "))
toolbar.AddControl(self.width_spinner)
# Implicit transition display
toggle_all = wx.ToggleButton(toolbar,-1,'Show Implicit')
toggle_all.Bind(wx.EVT_TOGGLEBUTTON, self.toggle_all_transitions)
self._show_all_transitions = False
toolbar.AddControl(wx.StaticText(toolbar,-1," "))
toolbar.AddControl(toggle_all)
toggle_auto_focus = wx.ToggleButton(toolbar, -1, 'Auto Focus')
toggle_auto_focus.Bind(wx.EVT_TOGGLEBUTTON, self.toggle_auto_focus)
self._auto_focus = False
toolbar.AddControl(wx.StaticText(toolbar, -1, " "))
toolbar.AddControl(toggle_auto_focus)
toolbar.AddControl(wx.StaticText(toolbar,-1," "))
toolbar.AddLabelTool(wx.ID_HELP, 'Help',
wx.ArtProvider.GetBitmap(wx.ART_HELP,wx.ART_OTHER,(16,16)) )
toolbar.AddLabelTool(wx.ID_SAVE, 'Save',
wx.ArtProvider.GetBitmap(wx.ART_FILE_SAVE,wx.ART_OTHER,(16,16)) )
toolbar.Realize()
self.Bind(wx.EVT_TOOL, self.ShowControlsDialog, id=wx.ID_HELP)
self.Bind(wx.EVT_TOOL, self.SaveDotGraph, id=wx.ID_SAVE)
# Create dot graph widget
self.widget = xdot.wxxdot.WxDotWindow(graph_view, -1)
gv_vbox.Add(toolbar, 0, wx.EXPAND)
gv_vbox.Add(self.widget, 1, wx.EXPAND)
# Create tree view widget
self.tree = wx.TreeCtrl(nb,-1,style=wx.TR_HAS_BUTTONS)
nb.AddPage(graph_view,"Graph View")
nb.AddPage(self.tree,"Tree View")
# Create userdata widget
borders = wx.LEFT | wx.RIGHT | wx.TOP
border = 4
self.ud_win = wx.ScrolledWindow(self.content_splitter, -1)
self.ud_gs = wx.BoxSizer(wx.VERTICAL)
self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Path:"),0, borders, border)
self.path_input = wx.ComboBox(self.ud_win,-1,style=wx.CB_DROPDOWN)
self.path_input.Bind(wx.EVT_COMBOBOX,self.selection_changed)
self.ud_gs.Add(self.path_input,0,wx.EXPAND | borders, border)
self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Userdata:"),0, borders, border)
self.ud_txt = wx.TextCtrl(self.ud_win,-1,style=wx.TE_MULTILINE | wx.TE_READONLY)
self.ud_gs.Add(self.ud_txt,1,wx.EXPAND | borders, border)
# Add initial state button
self.is_button = wx.Button(self.ud_win,-1,"Set as Initial State")
self.is_button.Bind(wx.EVT_BUTTON, self.on_set_initial_state)
self.is_button.Disable()
self.ud_gs.Add(self.is_button,0,wx.EXPAND | wx.BOTTOM | borders, border)
self.ud_win.SetSizer(self.ud_gs)
# Set content splitter
self.content_splitter.SplitVertically(viewer, self.ud_win, 512)
# Add statusbar
self.statusbar = wx.StatusBar(self,-1)
# Add elements to sizer
vbox.Add(self.content_splitter, 1, wx.EXPAND | wx.ALL)
vbox.Add(self.statusbar, 0, wx.EXPAND)
self.SetSizer(vbox)
self.Center()
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._containers= {}
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
self.Bind(wx.EVT_IDLE,self.OnIdle)
self.Bind(wx.EVT_CLOSE,self.OnQuit)
# Register mouse event callback
self.widget.register_select_callback(self.select_cb)
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
# Start a thread in the background to update the server list
self._keep_running = True
self._server_list_thread = threading.Thread(target=self._update_server_list)
self._server_list_thread.start()
self._update_graph_thread = threading.Thread(target=self._update_graph)
self._update_graph_thread.start()
self._update_tree_thread = threading.Thread(target=self._update_tree)
self._update_tree_thread.start()
def __init__(self):
# bot name
self.all_state_list = ['attack', 'escape', 'disturb']
# Message subscribers
self._structure_subs = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._containers= {}
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# before camera result
self._prev_camera_result = [0.0, 0.0]
# sub
"""
use almost all the our publish data
- enemy_pos_from_score
- pub_score
- enemy_pos_from_lider
- rem_time
- my_pose
- enemy_pose_from_camera
present status
-
"""
self.sub_enemy_pos_from_score = rospy.Subscriber('/enemy_pos_from_score', Float32MultiArray, self.enemy_pos_from_score_callback)
self.enemy_pos_from_score=Float32MultiArray()
self.sub_score = rospy.Subscriber('/score',Int32MultiArray,self.score_callback)
self.score = []
self.sub_enemy_pos_from_lider = rospy.Subscriber('/enemy_pos_from_lider', Point, self.enemy_pos_from_lider_callback)
self.enemy_pos_from_lider={"enemy_pos":Point(),"is_topic_receive":False}
self.sub_rem_time=rospy.Subscriber('rem_time',Time,self.rem_time_callback)
self.rem_time = Time()
self.sub_my_pose = rospy.Subscriber('/my_pose', MyPose, self.my_pose_callback)
self.my_pose = MyPose()
self.sub_enemy_pose_from_camera = rospy.Subscriber('/enemy_pose_from_camera', MyPose, self.enemy_pose_from_camera_callback)
self.enemy_pose_from_camera = MyPose()
self.current_state = None
# pub
self.pub_strategy = rospy.Publisher('/strategy', String, queue_size=1)
self.pub_state_stop = rospy.Publisher('/state_stop', Bool, queue_size=1)
# thread
# Start a thread in the background to update the server list
self._keep_running = True
self._server_list_thread = threading.Thread(target=self._update_server_list)
self._server_list_thread.start()
# Start a thread in the background to check stagnation
self._stagnation = False
self._check_stagnation_thread = threading.Thread(target=self._check_stagnation)
self._check_stagnation_thread.start()
