def __init__(self, is_dummy_action=False):
"""Initialization."""
self.is_dummy_action = is_dummy_action
self.ema_speed = 1.0 # avoid sending key at start
self.speed = 0.0
self.lock = Event()
self.lock.clear()
self.lock1 = Event()
self.lock1.clear()
# ROS related
rospy.init_node('car_go')
rospy.Subscriber('/car/control', Control, self.car_control_callback)
rospy.Subscriber('/car/status', CarStatus, self.car_status_callback)
self.start_pub = rospy.Publisher('/autoDrive_KeyboardMode',
Char,
queue_size=10,
latch=True)
# self.car_go_loop = Timer(rospy.Duration(5.0), self.car_go_callback)
self.ema_speed_loop = Timer(rospy.Duration(0.1),
self.ema_speed_callback)
if self.is_dummy_action:
print "[CarGo]: using dummy action, forming loop.."
self.dummy_action_loop = Timer(
rospy.Duration(5.0),
lambda *args, **kwargs: self.start_pub.publish(ord('0')))
def __init__(self, reset_to, reset_speed, reset_radius, fences):
"""Initialization.
"""
# === Init super class ===
super(ToyCarEpisodeMonitor, self).__init__()
# === Subprocess related ===
self.process_list = list()
self.process_names = [
['roslaunch', 'control',
'toycar_control.launch'], # launch control node when resetting
]
self.lock_autodrive_mode = Event() # lock sendding '\32'
self.lock_autodrive_mode.clear()
self.lock_perimeter = Event() # lock checking perimeter
self.lock_perimeter.set()
# === Reset config ===
self.param_reset_to = reset_to
self.param_reset_speed = reset_speed
self.param_reset_radius = reset_radius
self.param_fences = fences
# === Simulator states ===
self.last_autodrive_mode = None
self.car_status = None
self.car_x_ema = None
self.car_y_ema = None
self.car_hdg_ema = None
# == Additional publishers and subscribers ===
self.pub_traj_plan = rospy.Publisher("/planning/trajectory",
PlanningTraj,
queue_size=1,
latch=True)
self.pub_traj_viz = rospy.Publisher("/visualization/trajectory",
Path,
queue_size=1,
latch=True)
self.pub_keyboardmode = rospy.Publisher('/autoDrive_KeyboardMode',
Char,
queue_size=1,
latch=True)
self.pub_control = rospy.Publisher('/car/control',
Control,
queue_size=1,
latch=True)
self.sub_car_status = rospy.Subscriber('/car/status', CarStatus,
self._log_car_status)
self.sub_car_control = rospy.Subscriber('/car/control', Control,
self._log_autodrive_mode)
self.perimeter_checker = Timer(rospy.Duration(0.1),
self._check_perimeter)
self.control_mounter = Timer(rospy.Duration(1.0), self._mount_control)
def start(self):
if not self.__running:
self.__get_history()
self.__register_subscribers()
self.__running = True
self.__timer = Timer(Duration(nsecs=UPDATE_FREQUENCY),
self.__update_model)
def __init__(self, logger, seuid, first_message, parent=None):
"""Initializes the TopicItem.
:param seuid: the seuid of the item
:type seuid: str
:param logger: a logger where to log when special events occur
:type logger: ModelLogger
:param parent: the parent-item
:type parent: AbstractItem
"""
AbstractItem.__init__(self, logger, seuid, parent)
self.__parent = parent
self._type = "topic"
self.add_keys = ["dropped_msgs", "traffic", "bandwidth", "frequency"]
self.avg_keys = [
"period_mean", "period_stddev", "stamp_age_mean",
"stamp_age_stddev"
]
self.max_keys = ["period_max", "stamp_age_max"]
self._attributes = []
self._attributes.extend([
"dropped_msgs", "traffic", "period_mean", "period_stddev",
"period_max", "stamp_age_mean", "stamp_age_stddev",
"stamp_age_max", "bandwidth", "frequency"
])
for item in self._attributes:
self._add_data_list(item)
self.__calculated_data = {}
for key in self._attributes:
self.__calculated_data[key] = []
self.__calculated_data["window_stop"] = []
self.__calculated_data["window_start"] = []
for item in self._attributes:
self._rated_attributes.append(item + ".actual_value")
self._rated_attributes.append(item + ".expected_value")
self._rated_attributes.append(item + ".state")
for item in self._rated_attributes:
self._add_rated_data_list(item)
self._logger.log("info", Time.now(), seuid, "Created a new TopicItem")
self.__timer = Timer(Duration(nsecs=TOPIC_AGGREGATION_FREQUENCY),
self.__aggregate_topic_data)
self.tree_items = []
self.__aggregation_window = rospy.get_param("~aggregation_window", 5)
### CARSON ADDED ###o
self.throttle = None
def __init__(self, node_names, rate, topic_prefix='/rl/node_up'):
self._node_names = node_names
self._rate = rate
self._state = {name: None for name in self._node_names}
rospy.init_node('node_monitor')
self.pubs = {
name: rospy.Publisher(topic_prefix + name,
Bool,
queue_size=10,
latch=True)
for name in self._node_names
}
self.monitor_loop = Timer(
rospy.Duration(nsecs=int(1.0 / self._rate * 1e9)),
self.monitor_once)
def __init__(self, *args, **kwargs):
rospy.init_node('EpisodeMonitor')
# Subscribers
rospy.Subscriber('/rl/simulator_restart', Bool, self.restart_callback)
# Publishers
self.is_running = False
# This is an event-driven state signal
self.is_running_pub = rospy.Publisher("/rl/is_running",
Bool,
queue_size=10,
latch=True)
# This is periodic `episode_done` signal
self.heartbeat_pub = rospy.Publisher("/rl/simulator_heartbeat",
Bool,
queue_size=10,
latch=True)
Timer(rospy.Duration(1 / 20.0), self.__heartbeat)
def __init__(self):
"""
Initializes the widget.
"""
super(OverviewWidget, self).__init__()
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_overview_plugin'), 'resources', 'OverviewWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.__draw_graphs = False
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__last_update = rospy.Time.now()
self.__model = ROSModel()
self.__log_filter_proxy = LogFilterProxy()
self.__logger = self.__model.get_logger()
self.__style_string = ".detailed_data_overview {\n" \
" font-size: 13\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
#todo: are these in the right order?
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
#todo: adapt time!!!
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.__log_filter_proxy.filter_by_item(None)
self.__log_filter_proxy.setDynamicSortFilter(True)
#set proxy model
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.log_tab_tree_view.setRootIsDecorated(True)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__connect_slots()
self.__state = "ok"
self.__previous_state = "ok"
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.__last_update = rospy.Time.now()
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = self.__model.get_root_item().get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__first_resize = True
self.__plotted_curves = {}
self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
class OverviewWidget(QWidget):
"""
The overviewWidget of the ArniGuiOverview-Plugin.
"""
def __init__(self):
"""
Initializes the widget.
"""
super(OverviewWidget, self).__init__()
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_overview_plugin'), 'resources', 'OverviewWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.__draw_graphs = False
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__last_update = rospy.Time.now()
self.__model = ROSModel()
self.__log_filter_proxy = LogFilterProxy()
self.__logger = self.__model.get_logger()
self.__style_string = ".detailed_data_overview {\n" \
" font-size: 13\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
#todo: are these in the right order?
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
#todo: adapt time!!!
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.__log_filter_proxy.filter_by_item(None)
self.__log_filter_proxy.setDynamicSortFilter(True)
#set proxy model
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.log_tab_tree_view.setRootIsDecorated(True)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__connect_slots()
self.__state = "ok"
self.__previous_state = "ok"
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.__last_update = rospy.Time.now()
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = self.__model.get_root_item().get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__first_resize = True
self.__plotted_curves = {}
self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
def __del__(self):
"""
Destructor of the widget.
"""
self.__draw_graphs = False
self.__timer.stop()
del self.__timer
def create_graphs(self):
"""
Creates the graphs for the plot.
"""
self.__update_graphs_lock.acquire()
first_iteration = True
first_view = None
i = 0
self.__expected_items_per_group = 0
self.__graph_layout.clear()
for key in self.__plotable_items[min(self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):
min((self.__current_selected_combo_box_index + 1)
* self.__items_per_group, len(self.__plotable_items))]:
plot_widget = None
if first_iteration:
first_iteration = False
date_axis = DateAxis(orientation="bottom")
first_view = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(title=self.tr(key), axisItems={'bottom': date_axis}, viewBox=first_view)
else:
date_axis = DateAxis(orientation="bottom")
view_box = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(title=self.tr(key), viewBox=view_box, axisItems={'bottom': date_axis})
view_box.setXLink(first_view)
#performance enhancements when only a short range of the plot is shown
#plot_widget.setClipToView(clip=True)
plot_widget.setYRange(-1, 1)
self.__graph_dict[key] = plot_widget
self.__graph_layout.nextRow()
plot_widget = self.__graph_dict[key]
plot_widget.showGrid(x=True, y=True)
plot_widget.setMenuEnabled(enableMenu=True)
plot_widget.enableAutoRange('xy', True)
x = np.array([1])
y = np.array([int(str(Time.now()))/1000000000])
self.__plotted_curves[key] = plot_widget.plot(x=x, y=y, fillLevel=0, brush=(50, 50, 200, 100),
pen=(255, 0, 0))
self.__expected_items_per_group += 1
self.__first_update_pending = True
self.__update_graphs_lock.release()
def __connect_slots(self):
"""
Connects the slots.
"""
self.tab_widget.currentChanged.connect(self.__on_current_tab_changed)
self.range_combo_box.currentIndexChanged.connect(self.__on_range_combo_box_index_changed)
self.__model.layoutChanged.connect(self.update)
self.pause_button.clicked.connect(self.__on_pause_button_clicked)
self.selected_combo_box.currentIndexChanged.connect(self.__on_selected_combo_box_index_changed)
def __on_graph_window_size_changed(self):
# getting the size
size = self.__graph_layout.size()
items_per_group = max(int(math.ceil((size.height() - 100) / 200 + 1)), 1)
if items_per_group is not self.__items_per_group or self.__first_resize:
self.__first_resize = False
self.__graph_layout.set_blocked(True)
self.__items_per_group = 1 if items_per_group < 1 else items_per_group
self.__number_of_groups = int(math.ceil(len(self.__plotable_items) / float(self.__items_per_group)))
# change the groups in the widget
self.selected_combo_box.clear()
for group in range(0, self.__number_of_groups):
list = self.__plotable_items[min(group *
self.__items_per_group, len(self.__plotable_items)):min((group + 1)
* self.__items_per_group, len(self.__plotable_items))]
content = ""
for i in range(0, len(list) - 1):
content += self.tr(list[i])
content += ", "
content += list[len(list) - 1]
self.selected_combo_box.addItem(content)
# redraw
self.create_graphs()
self.update_graphs(None)
self.__graph_layout.set_blocked(False)
def __on_selected_combo_box_index_changed(self, index):
"""
Updates what is shown in the graphs
:param index: the index of the selected range
:type index: int
"""
if index is not -1:
self.__current_selected_combo_box_index = index
self.create_graphs()
self.update_graphs(None)
def __on_pause_button_clicked(self):
"""
To be called whenever the pause button is clicked. Stops the graphs from updating until the pause button
is clicked again and the other way.
"""
if self.__draw_graphs:
self.__draw_graphs = False
self.pause_button.setText(self.tr("Continue"))
else:
self.__draw_graphs = True
self.pause_button.setText(self.tr("Pause"))
def __on_current_tab_changed(self, tab):
"""
The Plugin wants to get notified when the tab changed so it can e.g. draw the graphs.
:param tab: the index of the selected tab
:type tab: int
"""
if tab is 1:
if self.pause_button.text() is not "Continue":
self.__draw_graphs = True
else:
self.__draw_graphs = False
else:
self.__draw_graphs = False
def __on_range_combo_box_index_changed(self, index):
"""
Handels the change of the graph range.
:param index: the index of the selected range
:type index: int
"""
self.__current_range_combo_box_index = index
def update(self):
"""
Updates the Plugin and draws the graphs if draw_graphs is true.
"""
data_dict = self.__model.get_root_item().get_latest_data("state")
self.__state = data_dict["state"]
if self.__previous_state is not self.__state:
self.__previous_state = self.__state
if self.__state == "ok":
self.status_text_line_edit.setText(self.tr("Current status: Ok"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_overview_plugin'), 'resources/graphics',
'light_green.png'))
elif self.__state == "warning":
self.status_text_line_edit.setText(self.tr("Current status: Warning"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_overview_plugin'), 'resources/graphics',
'light_orange.png'))
else:
self.status_text_line_edit.setText(self.tr("Current status: Error"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_overview_plugin'), 'resources/graphics',
'light_red.png'))
self.status_light_label.setPixmap(pixmap)
if self.information_tab_text_browser:
scroll_value = self.information_tab_text_browser.verticalScrollBar().value()
self.information_tab_text_browser.setHtml(self.__model.get_overview_text())
self.information_tab_text_browser.verticalScrollBar().setSliderPosition(scroll_value)
def update_graphs(self, event):
"""
Updates and redraws the graphs.
"""
self.__update_graphs_lock.acquire()
if self.__draw_graphs or self.__first_update_pending:
plotable_items = self.__plotable_items[min(self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min((self.__current_selected_combo_box_index + 1)
* self.__items_per_group, len(self.__plotable_items))]
plotable_data = self.__model.get_root_item().get_items_younger_than(
Time.now() - (Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)) if int(Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)).to_sec()) <= int(Time.now().to_sec()) else Time(0) ),
"window_stop", *plotable_items)
temp_time = []
temp_content = []
if plotable_data["window_stop"]:
modulo = (len(plotable_data["window_stop"]) / 200) + 1
length = len(plotable_data["window_stop"])
else:
length = 0
modulo = 1
for i in range(0, length, modulo):
# now having maximally 100 items to plot :)
temp_time.append(int(str(plotable_data["window_stop"][i]))/1000000000)
x = np.array(temp_time)
for key in plotable_items:
for i in range(0, length, modulo):
temp_content.append(plotable_data[key][i])
y = np.array(temp_content)
del temp_content[:]
self.__plotted_curves[key].setData(x=x, y=y)
self.__first_update_pending = False
self.__update_graphs_lock.release()
def __combo_box_index_to_seconds(self, index):
"""
Calculates the range from the combo-box index.
:param index: the index of teh combo-box
:type index: int
:returns: the seconds of the selected index
:rtype: int
"""
if self.__current_range_combo_box_index == 0:
return 10
elif self.__current_range_combo_box_index == 1:
return 30
else:
return 60
def get_current_tab(self):
"""
Returns the current tab.
:returns: the current tab
:rtype: int
"""
return self.tab_widget.currentIndex()
def set_current_tab(self, index=0):
"""
Sets the default tab.
:param index: the index of the tab
:type index: int
"""
if index is None:
index = 0
self.tab_widget.setCurrentIndex(index)
# WARNING: PROBABLY DOUBLE CALL OF __ON_CURRENT_TAB_CHANGED HERE BUT CANNOT BE FIXED SO EASILY
self.__on_current_tab_changed(index)
def get_range_combo_box_index(self):
"""
Returns the index of the combo-box.
:returns: the index
:rtype: int
"""
return self.range_combo_box.currentIndex()
def set_range_combo_box_index(self, index=0):
"""
Sets the default value of the combo-box.
:param index: the index of the combo-box
:type index: int
"""
if index is None:
index = 0
self.range_combo_box.setCurrentIndex(index)
def __init__(self, model):
"""
Initializes the Widget.
:param model: the model of the widget
:type model: ROSModel
"""
super(SelectionWidget, self).__init__()
self.setObjectName('selection_widget')
self.__model = model
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.setObjectName('SelectionWidgetUi')
self.__selected_item = None
self.__draw_graphs = True
self.__current_combo_box_index = 0
self.__last_update = rospy.Time.now()
try:
if rospy.get_param("/enable_statistics") == False:
raise KeyError('/enable_statistics')
except KeyError:
self.__overview_widget = None
raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
"roscore with the neccesary parameters or to load these while running (see init_params.launch)")
self.__values_dict = {
"bandwidth_mean": 0,
"bandwidth_stddev": 0,
"bandwidth_max": 0,
}
self.__logger = self.__model.get_logger()
self.__log_filter_proxy = LogFilterProxy()
self.__log_filter_proxy.filter_by_item(self.__selected_item)
self.__log_filter_proxy.setDynamicSortFilter(True)
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__style_string = ".detailed_data {\n" \
" font-size: 12\n;" \
"}\n"
self.__style_string = ".erroneous_entry {\n" \
" color: red\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
#self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.tab_widget.setTabText(3, self.tr("Actions"))
self.stop_push_button.setText(self.tr("Stop Node"))
self.restart_push_button.setText(self.tr("Restart Node"))
self.stop_push_button.setEnabled(False)
self.restart_push_button.setEnabled(False)
### CARSON ADDED ###
# set default values for throttle rate and window sliders
self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_rate_slider.setValue(5000)
self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_window_slider.setValue(500)
# set up validator for throttle rate and window text fields
# only allows floating point numbers
regex = QRegExp(r'[0-9]*\.?[0-9]+')
validator = QRegExpValidator(regex)
self.throttle_rate.setValidator(validator)
self.throttle_window.setValidator(validator)
# set up QButtonGroup for message/bandwidth throttle type radio buttons
self.throttle_radio_group = QButtonGroup()
self.throttle_radio_group.addButton(self.throttle_message_radio)
self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
### ###
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.log_tab_tree_view.setRootIsDecorated(False)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.set_selected_item(self.__selected_item)
self.__model.layoutChanged.connect(self.update)
self.__state = "ok"
self.__previous_state = "ok"
self.__selected_item_changed = True
self.__deleted = False
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = None#self.__selected_item.get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__plotted_curves = {}
#self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
def run(self):
"""Run the simulator backend for one episode.
This process target method run the simulator backend for one episode.
It initializes nodes to listen to simulator topics and publish action
and restart signals to the simulation.
Handshake procedures:
1. Setup listener and sender nodes.
2. Signal simulator restart and wait for new observation
(with timeout).
3. Send space, 1, g to /autoDrive_KeyboadMode to start the car.
4. Action sender doesn't send keys until simulator is up and car is
started (to avoid flooding signals in step 3).
5. Heartbeat checker listen to simulator heartbeat (async) and set
is_envnode_terminatable flag to terminate this process (i.e. poison pill).
"""
print "[EnvNode]: started frontend process: {}".format(self.name)
# === Specify experience preparation functions ===
# TODO: should be able to specify arbitrary ways to prepare exp
self.list_prep_exp = []
for i in range(len(self.defs_obs)):
if i<1:
self.list_prep_exp.append(self.__prep_image)
else:
self.list_prep_exp.append(self.__prep_reward)
self.list_prep_exp += [self.__prep_reward]*len(self.defs_reward)
# === Setup sync events ===
self.is_receiving_obs.clear()
self.first_time.set()
# === Initialize frontend ROS node ===
print "[EnvNode]: initialiting node..."
rospy.init_node('DrivingSimulatorEnv')
self.brg = CvBridge()
# obs + Reward subscribers (synchronized)
f_subs = lambda defs: message_filters.Subscriber(defs[0], defs[1])
self.ob_subs = map(f_subs, self.defs_obs)
self.reward_subs = map(f_subs, self.defs_reward)
self.ts = message_filters.ApproximateTimeSynchronizer(
self.ob_subs + self.reward_subs, 10, 0.1, allow_headerless=True)
self.ts.registerCallback(self.__enque_exp)
# heartbeat subscribers
rospy.Subscriber('/rl/simulator_heartbeat', Bool, self.__enque_done)
rospy.Subscriber('/rl/is_running', Bool, self.__heartbeat_checker)
# action and restart publishers
f_pubs = lambda defs: rospy.Publisher(
defs[0], defs[1], queue_size=100, latch=True)
self.action_pubs = map(f_pubs, self.defs_action)
# publish action periodically if not using dummy action
if not self.is_dummy_action:
self.actor_loop = Timer(
rospy.Duration(1.0/self.rate_action), self.__take_action)
self.restart_pub = rospy.Publisher(
'/rl/simulator_restart', Bool, queue_size=10, latch=True)
print "[EnvNode]: node initialized."
# === Simulator initialization: ===
# 1. signal start
# 2. wait for new observation and count cnt_fail seconds
# 3. mark initialization failed and break upon timeout
# 4. mark initialization failed and break upon termination flag
print "[EnvNode]: signal simulator restart"
self.restart_pub.publish(True)
cnt_fail = self.NODE_TIMEOUT
flag_fail = False
while not self.is_receiving_obs.is_set() or self.first_time.is_set():
cnt_fail -= 1 if cnt_fail>=0 else 0
print "[EnvNode]: simulator not up, wait for {} sec(s)...".format(cnt_fail)
time.sleep(1.0)
if cnt_fail<=0 or self.is_envnode_terminatable.is_set():
self.restart_pub.publish(False)
print "[EnvNode]: simulation initialization failed, "
flag_fail = True
break
# === Run simulator ===
# 1. set `is_envnode_up` Event
# 2. Periodically check backend status and termination event
t = time.time()
if not flag_fail:
print "[EnvNode]: simulator up and receiving obs."
for i in range(2):
print "[EnvNode]: simulation start in {} secs.".format(i)
time.sleep(1.0)
self.is_envnode_resetting.clear()
self.is_envnode_up.set()
# Loop check if simulation episode is done
while self.is_backend_up.is_set() and \
not self.is_envnode_terminatable.is_set():
time.sleep(0.2)
else:
# if initialization failed terminate the front end
self.is_envnode_terminatable.set()
# shutdown frontend ROS threads
rospy.signal_shutdown('[DrivingSimulatorEnv]: simulator terminated.')
# Close queues for this process
for key in self.q:
self.q[key].close()
# Return from this process
print ("[EnvNode]: returning from run in process: "
"{} PID: {}, after {:.2f} secs...").format(
self.name, self.pid, time.time()-t)
secs = 2
while secs != 0:
print "..in {} secs".format(secs)
secs -= 1
time.sleep(1.0)
print "[EnvNode]: Now!"
# manually set this event to notify queue monitor to clear queues
self.is_envnode_terminatable.set()
return
def __init__(self):
"""
Initializes the widget.
"""
super(OverviewWidget, self).__init__()
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('rqt_arni_gui_overview'),
'resources', 'OverviewWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.__draw_graphs = False
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__last_update = rospy.Time.now()
self.__model = ROSModel()
self.__log_filter_proxy = LogFilterProxy()
self.__logger = self.__model.get_logger()
self.__style_string = ".detailed_data_overview {\n" \
" font-size: 13\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
#todo: are these in the right order?
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
#todo: adapt time!!!
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.__log_filter_proxy.filter_by_item(None)
self.__log_filter_proxy.setDynamicSortFilter(True)
#set proxy model
self.__log_filter_proxy.setSourceModel(
self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.log_tab_tree_view.setRootIsDecorated(True)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__connect_slots()
self.__state = "ok"
self.__previous_state = "ok"
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.__last_update = rospy.Time.now()
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(
self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = self.__model.get_root_item(
).get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__first_resize = True
self.__plotted_curves = {}
self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
class OverviewWidget(QWidget):
"""
The overviewWidget of the ArniGuiOverview-Plugin.
"""
def __init__(self):
"""
Initializes the widget.
"""
super(OverviewWidget, self).__init__()
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('rqt_arni_gui_overview'),
'resources', 'OverviewWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.__draw_graphs = False
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__last_update = rospy.Time.now()
self.__model = ROSModel()
self.__log_filter_proxy = LogFilterProxy()
self.__logger = self.__model.get_logger()
self.__style_string = ".detailed_data_overview {\n" \
" font-size: 13\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
#todo: are these in the right order?
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
#todo: adapt time!!!
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.__log_filter_proxy.filter_by_item(None)
self.__log_filter_proxy.setDynamicSortFilter(True)
#set proxy model
self.__log_filter_proxy.setSourceModel(
self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.log_tab_tree_view.setRootIsDecorated(True)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__connect_slots()
self.__state = "ok"
self.__previous_state = "ok"
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.__last_update = rospy.Time.now()
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(
self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = self.__model.get_root_item(
).get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__first_resize = True
self.__plotted_curves = {}
self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
def __del__(self):
"""
Destructor of the widget.
"""
self.__draw_graphs = False
self.__timer.stop()
del self.__timer
def create_graphs(self):
"""
Creates the graphs for the plot.
"""
self.__update_graphs_lock.acquire()
first_iteration = True
first_view = None
i = 0
self.__expected_items_per_group = 0
self.__graph_layout.clear()
for key in self.__plotable_items[min(
self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min(
(self.__current_selected_combo_box_index + 1) *
self.__items_per_group, len(self.__plotable_items))]:
plot_widget = None
if first_iteration:
first_iteration = False
date_axis = DateAxis(orientation="bottom")
first_view = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(
title=self.tr(key),
axisItems={'bottom': date_axis},
viewBox=first_view)
else:
date_axis = DateAxis(orientation="bottom")
view_box = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(
title=self.tr(key),
viewBox=view_box,
axisItems={'bottom': date_axis})
view_box.setXLink(first_view)
#performance enhancements when only a short range of the plot is shown
#plot_widget.setClipToView(clip=True)
plot_widget.setYRange(-1, 1)
self.__graph_dict[key] = plot_widget
self.__graph_layout.nextRow()
plot_widget = self.__graph_dict[key]
plot_widget.showGrid(x=True, y=True)
plot_widget.setMenuEnabled(enableMenu=True)
plot_widget.enableAutoRange('xy', True)
x = np.array([1])
y = np.array([int(str(Time.now())) / 1000000000])
self.__plotted_curves[key] = plot_widget.plot(x=x,
y=y,
fillLevel=0,
brush=(50, 50, 200,
100),
pen=(255, 0, 0))
self.__expected_items_per_group += 1
self.__first_update_pending = True
self.__update_graphs_lock.release()
def __connect_slots(self):
"""
Connects the slots.
"""
self.tab_widget.currentChanged.connect(self.__on_current_tab_changed)
self.range_combo_box.currentIndexChanged.connect(
self.__on_range_combo_box_index_changed)
self.__model.layoutChanged.connect(self.update)
self.pause_button.clicked.connect(self.__on_pause_button_clicked)
self.selected_combo_box.currentIndexChanged.connect(
self.__on_selected_combo_box_index_changed)
def __on_graph_window_size_changed(self):
# getting the size
size = self.__graph_layout.size()
items_per_group = max(int(math.ceil((size.height() - 100) / 200 + 1)),
1)
if items_per_group is not self.__items_per_group or self.__first_resize:
self.__first_resize = False
self.__graph_layout.set_blocked(True)
self.__items_per_group = 1 if items_per_group < 1 else items_per_group
self.__number_of_groups = int(
math.ceil(
len(self.__plotable_items) /
float(self.__items_per_group)))
# change the groups in the widget
self.selected_combo_box.clear()
for group in range(0, self.__number_of_groups):
list = self.__plotable_items[min(
group *
self.__items_per_group, len(self.__plotable_items)):min(
(group + 1) *
self.__items_per_group, len(self.__plotable_items))]
content = ""
for i in range(0, len(list) - 1):
content += self.tr(list[i])
content += ", "
content += list[len(list) - 1]
self.selected_combo_box.addItem(content)
# redraw
self.create_graphs()
self.update_graphs(None)
self.__graph_layout.set_blocked(False)
def __on_selected_combo_box_index_changed(self, index):
"""
Updates what is shown in the graphs
:param index: the index of the selected range
:type index: int
"""
if index is not -1:
self.__current_selected_combo_box_index = index
self.create_graphs()
self.update_graphs(None)
def __on_pause_button_clicked(self):
"""
To be called whenever the pause button is clicked. Stops the graphs from updating until the pause button
is clicked again and the other way.
"""
if self.__draw_graphs:
self.__draw_graphs = False
self.pause_button.setText(self.tr("Continue"))
else:
self.__draw_graphs = True
self.pause_button.setText(self.tr("Pause"))
def __on_current_tab_changed(self, tab):
"""
The Plugin wants to get notified when the tab changed so it can e.g. draw the graphs.
:param tab: the index of the selected tab
:type tab: int
"""
if tab is 1:
if self.pause_button.text() is not "Continue":
self.__draw_graphs = True
else:
self.__draw_graphs = False
else:
self.__draw_graphs = False
def __on_range_combo_box_index_changed(self, index):
"""
Handels the change of the graph range.
:param index: the index of the selected range
:type index: int
"""
self.__current_range_combo_box_index = index
def update(self):
"""
Updates the Plugin and draws the graphs if draw_graphs is true.
"""
data_dict = self.__model.get_root_item().get_latest_data("state")
self.__state = data_dict["state"]
if self.__previous_state is not self.__state:
self.__previous_state = self.__state
if self.__state == "ok":
self.status_text_line_edit.setText(
self.tr("Current status: Ok"))
pixmap = QPixmap(
os.path.join(self.rp.get_path('rqt_arni_gui_overview'),
'resources/graphics', 'light_green.png'))
elif self.__state == "warning":
self.status_text_line_edit.setText(
self.tr("Current status: Warning"))
pixmap = QPixmap(
os.path.join(self.rp.get_path('rqt_arni_gui_overview'),
'resources/graphics', 'light_orange.png'))
else:
self.status_text_line_edit.setText(
self.tr("Current status: Error"))
pixmap = QPixmap(
os.path.join(self.rp.get_path('rqt_arni_gui_overview'),
'resources/graphics', 'light_red.png'))
self.status_light_label.setPixmap(pixmap)
if self.information_tab_text_browser:
scroll_value = self.information_tab_text_browser.verticalScrollBar(
).value()
self.information_tab_text_browser.setHtml(
self.__model.get_overview_text())
self.information_tab_text_browser.verticalScrollBar(
).setSliderPosition(scroll_value)
def update_graphs(self, event):
"""
Updates and redraws the graphs.
"""
self.__update_graphs_lock.acquire()
if self.__draw_graphs or self.__first_update_pending:
plotable_items = self.__plotable_items[min(
self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min(
(self.__current_selected_combo_box_index + 1) *
self.__items_per_group, len(self.__plotable_items))]
plotable_data = self.__model.get_root_item(
).get_items_younger_than(
Time.now() - (Duration(secs=self.__combo_box_index_to_seconds(
self.__current_range_combo_box_index)) if int(
Duration(secs=self.__combo_box_index_to_seconds(
self.__current_range_combo_box_index)).to_sec()) <=
int(Time.now().to_sec()) else Time(0)),
"window_stop", *plotable_items)
temp_time = []
temp_content = []
if plotable_data["window_stop"]:
modulo = (len(plotable_data["window_stop"]) / 200) + 1
length = len(plotable_data["window_stop"])
else:
length = 0
modulo = 1
for i in range(0, length, modulo):
# now having maximally 100 items to plot :)
temp_time.append(
int(str(plotable_data["window_stop"][i])) / 1000000000)
x = np.array(temp_time)
for key in plotable_items:
for i in range(0, length, modulo):
temp_content.append(plotable_data[key][i])
y = np.array(temp_content)
del temp_content[:]
self.__plotted_curves[key].setData(x=x, y=y)
self.__first_update_pending = False
self.__update_graphs_lock.release()
def __combo_box_index_to_seconds(self, index):
"""
Calculates the range from the combo-box index.
:param index: the index of teh combo-box
:type index: int
:returns: the seconds of the selected index
:rtype: int
"""
if self.__current_range_combo_box_index == 0:
return 10
elif self.__current_range_combo_box_index == 1:
return 30
else:
return 60
def get_current_tab(self):
"""
Returns the current tab.
:returns: the current tab
:rtype: int
"""
return self.tab_widget.currentIndex()
def set_current_tab(self, index=0):
"""
Sets the default tab.
:param index: the index of the tab
:type index: int
"""
if index is None:
index = 0
self.tab_widget.setCurrentIndex(index)
# WARNING: PROBABLY DOUBLE CALL OF __ON_CURRENT_TAB_CHANGED HERE BUT CANNOT BE FIXED SO EASILY
self.__on_current_tab_changed(index)
def get_range_combo_box_index(self):
"""
Returns the index of the combo-box.
:returns: the index
:rtype: int
"""
return self.range_combo_box.currentIndex()
def set_range_combo_box_index(self, index=0):
"""
Sets the default value of the combo-box.
:param index: the index of the combo-box
:type index: int
"""
if index is None:
index = 0
self.range_combo_box.setCurrentIndex(index)
def __init__(self, duration=1, topic='tick'):
self.topic_name = topic
self.publisher = rospy.Publisher(topic, Clock, queue_size=10)
self.duration = rospy.Duration(duration)
self.timer = Timer(self.duration, self.send_tick)
