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, 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)
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)
