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)