def _load_realsense_gaits(self, gaits): """ Load all gaits from the realsense gait version map. Also create a service with a separate callback group that can be used by the realsense gaits to get parameters from the realsense_reader. A new callback group is necessary to prevent a deadlock. :param gaits: The dictionary where the loaded gaits will be added to. """ if not self._validate_inverse_kinematics_is_possible(): return get_gait_parameters_service = self.create_client( srv_type=GetGaitParameters, srv_name="/camera/process_pointcloud", callback_group=MutuallyExclusiveCallbackGroup(), ) for gait_name in self._realsense_gait_version_map: gait_folder = gait_name gait_path = os.path.join(self._gait_directory, gait_folder, gait_name + ".gait") with open(gait_path, "r") as gait_file: gait_graph = yaml.load(gait_file, Loader=yaml.SafeLoader)["subgaits"] gait = RealsenseGait.from_yaml( gait_selection=self, robot=self._robot, gait_name=gait_name, gait_config=self._realsense_gait_version_map[gait_name], gait_graph=gait_graph, gait_directory=self._gait_directory, process_service=get_gait_parameters_service, ) gaits[gait_name] = gait
def __init__(self): super().__init__("async_service_server") self.logger = self.get_logger() # add_two_intsサービスサーバーの作成 # (execute_callback実行は複数同時処理を許可) cg_re_01 = ReentrantCallbackGroup() cg_mu_01 = MutuallyExclusiveCallbackGroup() self._srv = self.create_service(AddTwoInts, "add_two_ints", callback=self.add_two_ints_callback, callback_group=cg_re_01 ) # ---------- Timer 01 ---------- self._timer01_cnt = 0 self._timer01 = self.create_timer(timer_period_sec=3.0, callback=self._on_timer01, callback_group=cg_mu_01) # ---------- Timer 01 ---------- self._timer02_cnt = 0 self._timer02 = self.create_timer(timer_period_sec=3.0, callback=self._on_timer02, callback_group=cg_mu_01) self._dtstr = self._get_time_now() self.list_len = 3 self._share_list = list(range(self._timer01_cnt, self._timer01_cnt + self.list_len)) self.logger.info('----- [service_server]Start! -----')
def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True): self._handle = None self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() namespace = namespace or '' if not ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node(node_name, namespace, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(self.handle))
def test_mutually_exclusive_group(self): self.assertIsNotNone(self.node.handle) group = MutuallyExclusiveCallbackGroup() t1 = self.node.create_timer(1.0, lambda: None, callback_group=group) t2 = self.node.create_timer(1.0, lambda: None, callback_group=group) self.assertTrue(group.can_execute(t1)) self.assertTrue(group.can_execute(t2)) self.assertTrue(group.beginning_execution(t1)) self.assertFalse(group.can_execute(t2)) self.assertFalse(group.beginning_execution(t2)) group.ending_execution(t1) self.assertTrue(group.can_execute(t2)) self.assertTrue(group.beginning_execution(t2))
def create_subscriber(self, msg_type, topic, callback, qos_profile=None, callback_group=None): if qos_profile is None: qos_profile = self.qos_profile if callback_group is None: callback_group = MutuallyExclusiveCallbackGroup() return self.create_subscription(msg_type, topic, callback, qos_profile, callback_group=callback_group)
def __init__(self): DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID') super().__init__("navigation_action_client", namespace = DRONE_DEVICE_ID) self._action_done_event = Event() self._action_cancel_event = Event() self._action_response_result = False self._action_cancel_result = False self._goal_handle = None self._action_callback_group = MutuallyExclusiveCallbackGroup() self._action_client = ActionClient(self, NavigateToPose, "/" + DRONE_DEVICE_ID + "/navigate_to_pose", callback_group = self._action_callback_group) self._service_callback_group = MutuallyExclusiveCallbackGroup() self._local_waypoint_service = self.create_service(Vec4, '~/local_waypoint', self.local_waypoint_callback, callback_group = self._service_callback_group) self._gps_waypoint_service = self.create_service(Vec4, '~/gps_waypoint', self.gps_waypoint_callback, callback_group = self._service_callback_group) self._cancel_goal = self.create_service(Trigger, '~/cancel_goal', self.cancel_goal_callback, callback_group = self._service_callback_group)
def __init__(self): super().__init__('rl_environment') """************************************************************** Initialize variables **************************************************************""" self.goal_pose_x = 0.0 self.goal_pose_y = 0.0 self.robot_pose_x = 0.0 self.robot_pose_y = 0.0 self.action_size = 5 self.done = False self.fail = False self.succeed = False self.time_out = 300 self.goal_angle = 0.0 self.goal_distance = 1.0 self.init_goal_distance = 0.25 self.scan_ranges = [] self.min_obstacle_distance = 10.0 self.local_step = 0 self.stop_cmd_vel_timer = None self.angular_vel = [1.4, 0.7, 0.0, -0.7, -1.4] self.action_reward = [-0.02, -0.015, 0.0, -0.015, -0.02] """************************************************************ Initialise publisher, subscribers, clients and services ************************************************************""" qos = QoSProfile(depth=10) # Initialize publisher self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos) # Initialize subscribers self.odom_sub = self.create_subscription(Odometry, 'odom', self.odom_sub_callback, qos) self.scan_sub = self.create_subscription(LaserScan, 'scan', self.scan_sub_callback, qos_profile_sensor_data) # Initialize client self.clients_callback_group = MutuallyExclusiveCallbackGroup() self.task_succeed_client = self.create_client( Goal, 'task_succeed', callback_group=self.clients_callback_group) self.task_failed_client = self.create_client( Goal, 'task_failed', callback_group=self.clients_callback_group) self.initialize_environment_client = self.create_client( Goal, 'initialize_env', callback_group=self.clients_callback_group) # Initialize service self.rl_agent_interface_service = self.create_service( Dqn, 'rl_agent_interface', self.rl_agent_interface_callback) self.make_environment_service = self.create_service( Empty, 'make_environment', self.make_environment_callback) self.reset_environment_service = self.create_service( Dqn, 'reset_environment', self.reset_environment_callback)
def create_service_client(self, service_name, service, timeout_sec=None, callback_group=None): if callback_group is None: callback_group = MutuallyExclusiveCallbackGroup() client = self.create_client(service, service_name, callback_group=callback_group) status = client.wait_for_service(timeout_sec=timeout_sec) if status is True: return client else: raise ROSException("Timeout of {}sec while waiting for service".format(timeout_sec))
def __init__( self, node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True, initial_parameters=None ): self._handle = None self._context = get_default_context() if context is None else context self._parameters = {} self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self.waitables = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node( node_name, namespace, self._context.handle, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle)) # Clock that has support for ROS time. # TODO(dhood): use sim time if parameter has been set on the node. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events) node_parameters = _rclpy.rclpy_get_node_parameters(Parameter, self.handle) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if initial_parameters is not None: node_parameters.update({p.name: p for p in initial_parameters}) self.set_parameters_atomically(node_parameters.values()) if start_parameter_services: self._parameter_service = ParameterService(self)
def __init__(self): super().__init__('minimal_client_async') self.cli = self.create_client(AddTwoInts, 'add_two_ints') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('service not available, waiting again...') self.req = AddTwoInts.Request() self.grp_timer = MutuallyExclusiveCallbackGroup() self.create_timer(1.0, self.send_request, callback_group=self.grp_timer)
def __init__(self): super().__init__('multi_thread_reciver') # Set logger lebel self.logger = super().get_logger() self.logger.set_level(rclpy.logging.LoggingSeverity.INFO) # ========== マルチスレッドテスト1 ========== cb_grp_ms_01 = ReentrantCallbackGroup() # Topic Recieve msg_type = String topic = "tpc_grp01_ms_snd" callback = self.on_tpc_grp01_ms_rcv self.sub_grp01_ms = self.create_subscription( msg_type=msg_type, topic=topic, callback=callback, callback_group=cb_grp_ms_01) # Topic Send self.pub_grp01_ms = self.create_publisher(String, "tpc_grp01_ms_rcv") # ========== マルチスレッドテスト2 ========== cb_grp_ree_01 = MutuallyExclusiveCallbackGroup() cb_grp_ree_02 = MutuallyExclusiveCallbackGroup() cb_grp_ree_03 = MutuallyExclusiveCallbackGroup() # String型のchatterトピックを送信するpublisherの定義 self.pub = self.create_publisher(String, 'tpc_test_rcv') self.srv01 = self.create_service(srv_type=AddTwoInts, srv_name="add_two_ints_01", callback=self.srv_callback01, callback_group=cb_grp_ree_01) self.srv02 = self.create_service(srv_type=AddTwoInts, srv_name="add_two_ints_02", callback=self.srv_callback02, callback_group=cb_grp_ree_02) self.srv03 = self.create_service(srv_type=AddTwoInts, srv_name="add_two_ints_03", callback=self.srv_callback03, callback_group=cb_grp_ree_03) self._dtstr = self.__get_time_now()
def __init__(self): super().__init__('rl_environment') self.train_mode = True self.goal_pose_x = 0.0 self.goal_pose_y = 0.0 self.robot_pose_x = 0.0 self.robot_pose_y = 0.0 self.action_size = 5 self.time_out = 1000000 # maximum number of actions in each episode self.done = False self.fail = False self.succeed = False # parameters to calculate the reward self.goal_angle = 0.0 self.goal_distance = 1.0 self.init_goal_distance = 0.1 self.scan_ranges = [] self.min_obstacle_distance = 10.0 self.local_step = 0 self.stop_cmd_vel_timer = None self.angular_vel = [1.0, 0.5, 0.0, -0.5, -1.0] qos = QoSProfile(depth=10) # Initialize publisher self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos) # Initialize subscribers self.odom_sub = self.create_subscription(Odometry, 'odom', self.odom_sub_callback, qos) self.scan_sub = self.create_subscription(LaserScan, '/turtlebot3_laserscan/out', self.scan_sub_callback, qos_profile_sensor_data) # Initialize client self.clients_callback_group = MutuallyExclusiveCallbackGroup() self.task_succeed_client = self.create_client( Goal, 'task_succeed', callback_group=self.clients_callback_group) self.task_failed_client = self.create_client( Goal, 'task_failed', callback_group=self.clients_callback_group) self.initialize_environment_client = self.create_client( Goal, 'initialize_env', callback_group=self.clients_callback_group) # Initialize service self.rl_agent_interface_service = self.create_service( Dqn, 'rl_agent_interface', self.rl_agent_interface_callback) self.make_environment_service = self.create_service( Empty, 'make_environment', self.make_environment_callback) self.reset_environment_service = self.create_service( Dqn, 'reset_environment', self.reset_environment_callback)
def __init__(self): super().__init__("multi_thread_sender") # Set logger lebel self.logger = super().get_logger() self.logger.set_level(rclpy.logging.LoggingSeverity.INFO) # ========== マルチスレッドテスト1 ========== self.cb_grp_ms_01_itr = 0 cb_grp_ms_01 = ReentrantCallbackGroup() # Topic Send self.pub_grp01_ms = self.create_publisher(String, "tpc_grp01_ms_snd") # Topic Recieve msg_type = String topic = "tpc_grp01_ms_rcv" callback = self.on_tpc_grp01_ms_rcv self.sub_grp01_ms = self.create_subscription( msg_type=msg_type, topic=topic, callback=callback, callback_group=cb_grp_ms_01) # Timer self.__timer_ms_grp01 = self.create_timer( timer_period_sec=3.0, callback=self.__on_timeout_ms_grp01, callback_group=cb_grp_ms_01) # ========== マルチスレッドテスト2 ========== self.cb_grp_ms_02_itr = 0 cb_grp_ms_02 = MutuallyExclusiveCallbackGroup() # Timer self.__timer_ms_grp02 = self.create_timer( timer_period_sec=3.0, callback=self.__on_timeout_ms_grp02, callback_group=cb_grp_ms_02) self.cli01 = self.create_client(AddTwoInts, "add_two_ints_01") while not self.cli01.wait_for_service(timeout_sec=1.0): self.get_logger().info("service not available, waiting again...") self.cli02 = self.create_client(AddTwoInts, "add_two_ints_02") while not self.cli02.wait_for_service(timeout_sec=1.0): self.get_logger().info("service not available, waiting again...") self.cli03 = self.create_client(AddTwoInts, "add_two_ints_03") while not self.cli03.wait_for_service(timeout_sec=1.0): self.get_logger().info("service not available, waiting again...") self.future01 = None self.future02 = None self.future03 = None self.tpc_seq_itr = 0 self.srv_seq_itr = 0 self._dtstr = self.__get_time_now()
def __init__(self, stage): super().__init__('gazebo_interface') """************************************************************** Initialize variables **************************************************************""" # Environment stage (could be 1, 2, 3, 4) self.stage = int(stage) # Read the 'Goal' Entity Model self.entity_name = 'Goal' self.entity = None self.open_entity() # initial entity(Goal) position self.entity_pose_x = 0.5 self.entity_pose_y = 0.0 """************************************************************* Initialize clients and services *************************************************************""" """ Initialize clients The following clients send their request to Gazebo simulator for - deleting entity (Goal) - spawn an entity (Goal) - reset the simulation environment """ self.delete_entity_client = self.create_client(DeleteEntity, 'delete_entity') self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity') self.reset_simulation_client = self.create_client( Empty, 'reset_simulation') # Initialize services """ Initialize services The following services give response to the request of their corresponding client in RLEnvironment class """ self.callback_group = MutuallyExclusiveCallbackGroup() self.initialize_env_service = self.create_service( Goal, 'initialize_env', self.initialize_env_callback, callback_group=self.callback_group) self.task_succeed_service = self.create_service( Goal, 'task_succeed', self.task_succeed_callback, callback_group=self.callback_group) self.task_failed_service = self.create_service( Goal, 'task_failed', self.task_failed_callback, callback_group=self.callback_group)
def __init__(self): super().__init__('double_talker') self.i = 0 self.pub = self.create_publisher(String, 'chatter', 10) # This type of callback group only allows one callback to be executed at a time self.group = MutuallyExclusiveCallbackGroup() # Pass the group as a parameter to give it control over the execution of the timer callback self.timer = self.create_timer(1.0, self.timer_callback, callback_group=self.group) self.timer2 = self.create_timer(0.5, self.timer_callback, callback_group=self.group)
def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True): self._handle = None self._parameters = {} self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None namespace = namespace or '' if not ok(): raise NotInitializedException('cannot create node') try: self._handle = _rclpy.rclpy_create_node(node_name, namespace, cli_args, use_global_arguments) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(self.handle)) # Clock that has support for ROS time. # TODO(dhood): use sim time if parameter has been set on the node. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None if start_parameter_services: self._parameter_service = ParameterService(self)
def __init__(self, stage): super().__init__('gazebo_interface') """************************************************************** Initialize variables **************************************************************""" # Environment stage (could be 1, 2, 3, 4) self.stage = int(stage) # Read the 'Goal' Entity Model self.entity_name = 'Goal' self.entity = None self.open_entity() self.entity_pose_x = 0.5 self.entity_pose_y = 0.0 """************************************************************* Initialize clients and services *************************************************************""" # Initialize clients self.delete_entity_client = self.create_client(DeleteEntity, 'delete_entity') self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity') self.reset_simulation_client = self.create_client( Empty, 'reset_simulation') # Initialize services self.callback_group = MutuallyExclusiveCallbackGroup() self.initialize_env_service = self.create_service( Goal, 'initialize_env', self.initialize_env_callback, callback_group=self.callback_group) self.task_succeed_service = self.create_service( Goal, 'task_succeed', self.task_succeed_callback, callback_group=self.callback_group) self.task_failed_service = self.create_service( Goal, 'task_failed', self.task_failed_callback, callback_group=self.callback_group)
def __init__(self, training): super().__init__('gazebo_interface') if training.lower() == 'false' or training == '0': self.training = False else: self.training = True self.consecutive_fails = 0 # Read the 'Goal' Entity Model self.entity_name = 'Goal' self.entity = open('./goal_box/model.sdf', 'r').read() # initial entity(Goal) position self.IndexCounter = 0 #Initialize clients self.delete_entity_client = self.create_client(DeleteEntity, 'delete_entity') self.spawn_entity_client = self.create_client(SpawnEntity, 'spawn_entity') self.reset_simulation_client = self.create_client( Empty, 'reset_simulation') # Initialize services self.callback_group = MutuallyExclusiveCallbackGroup() self.initialize_env_service = self.create_service( Goal, 'initialize_env', self.initialize_env_callback, callback_group=self.callback_group) self.task_succeed_service = self.create_service( Goal, 'task_succeed', self.task_succeed_callback, callback_group=self.callback_group) self.task_failed_service = self.create_service( Goal, 'task_failed', self.task_failed_callback, callback_group=self.callback_group)
def __init__(self): super().__init__("queue_server") self.logger = self.get_logger() self._NAME_IDX = "idx" self._NAME_DATA = "data" cg_re_01 = ReentrantCallbackGroup() cg_mu_01 = MutuallyExclusiveCallbackGroup() self._srv = self.create_service(GetParameters, "get_parameters", callback=self.get_parameters_callback, callback_group=cg_re_01) self._start_dt = dt.datetime.now() self._df = pd.DataFrame(columns=[self._NAME_IDX, self._NAME_DATA]) self._df.set_index(self._NAME_IDX, inplace=True) self._idx = 0 self.logger.info("----- [queue_server]Start! -----")
def __init__(self): """Create a ModelLoaderNode. """ super().__init__("model_loader_node") self.get_logger().info("model_loader_node started") self.models_in_progress = dict() # Threading lock object to safely perform the model operations. self.progress_guard = threading.Lock() # Scheduler to queue the function calls and run them in a separate thread. self.scheduler = scheduler.Scheduler(self.get_logger()) # Flag to enable deleting all existing models in /opt/aws/deepracer/artifacts folder # before copying the models from USB. self.enable_model_wipe = model_loader_config.ENABLE_MODEL_WIPE # Flag to enable model optimizer while transferring the models. Default set to False. if model_loader_config.ENABLE_MODEL_OPTIMIZER: self.model_optimizer_client = self.create_client( ModelOptimizeSrv, model_loader_config.MODEL_OPTIMIZER_SERVER_SERVICE) else: self.model_optimizer_client = None # Supported file extensions and their corresponding action functions. self.supported_exts = { ".pb": self.copymodel, ".json": self.copymodel, ".gz": self.unzip, ".tar": self.untar } # Supported model extension. self.model_file_extensions = (".pb") # Service that is called when a model is loaded to verify if the model # was extracted successfully. self.verify_model_ready_cb_group = ReentrantCallbackGroup() self.verify_model_ready_service = self.create_service( VerifyModelReadySrv, model_loader_config.VERIFY_MODEL_READY_SERVICE_NAME, self.verify_model_ready_cb, callback_group=self.verify_model_ready_cb_group) # A service that is called to extract a tar.gz file with model uploaded from the console # or delete a model selected. self.console_model_action_cb_group = ReentrantCallbackGroup() self.console_model_action_service = self.create_service( ConsoleModelActionSrv, model_loader_config.CONSOLE_MODEL_ACTION_SERVICE_NAME, self.console_model_action_cb, callback_group=self.console_model_action_cb_group) # Clients to Status LED services that are called to indicate progress/success/failure # status while loading model. self.led_cb_group = MutuallyExclusiveCallbackGroup() self.led_blink_client = self.create_client( SetStatusLedBlinkSrv, constants.LED_BLINK_SERVICE_NAME, callback_group=self.led_cb_group) self.led_solid_client = self.create_client( SetStatusLedSolidSrv, constants.LED_SOLID_SERVICE_NAME, callback_group=self.led_cb_group) while not self.led_blink_client.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led blink service not available, waiting again...") while not self.led_solid_client.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led solid service not available, waiting again...") self.led_blink_request = SetStatusLedBlinkSrv.Request() self.led_solid_request = SetStatusLedSolidSrv.Request() # Client to USB File system subscription service that allows the node to add the "models" # folder to the watchlist. The usb_monitor_node will trigger notification if it finds # the files/folders from the watchlist in the USB drive. self.usb_sub_cb_group = ReentrantCallbackGroup() self.usb_file_system_subscribe_client = self.create_client( USBFileSystemSubscribeSrv, constants.USB_FILE_SYSTEM_SUBSCRIBE_SERVICE_NAME, callback_group=self.usb_sub_cb_group) while not self.usb_file_system_subscribe_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "File System Subscribe not available, waiting again...") # Client to USB Mount point manager service to indicate that the usb_monitor_node can safely # decrement the counter for the mount point once the action function for the file/folder being # watched by model_loader_node is succesfully executed. self.usb_mpm_cb_group = ReentrantCallbackGroup() self.usb_mount_point_manager_client = self.create_client( USBMountPointManagerSrv, constants.USB_MOUNT_POINT_MANAGER_SERVICE_NAME, callback_group=self.usb_mpm_cb_group) while not self.usb_mount_point_manager_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "USB mount point manager service not available, waiting again..." ) # Subscriber to USB File system notification publisher to recieve the broadcasted messages # with file/folder details, whenever a watched file is identified in the USB connected. self.usb_notif_cb_group = ReentrantCallbackGroup() self.usb_file_system_notification_sub = self.create_subscription( USBFileSystemNotificationMsg, constants.USB_FILE_SYSTEM_NOTIFICATION_TOPIC, self.usb_file_system_notification_cb, 10, callback_group=self.usb_notif_cb_group) # Add the "models" folder to the watchlist. usb_file_system_subscribe_request = USBFileSystemSubscribeSrv.Request() usb_file_system_subscribe_request.file_name = model_loader_config.MODEL_SOURCE_LEAF_DIRECTORY usb_file_system_subscribe_request.callback_name = model_loader_config.SCHEDULE_MODEL_LOADER_CB usb_file_system_subscribe_request.verify_name_exists = True self.usb_file_system_subscribe_client.call_async( usb_file_system_subscribe_request) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback) self.get_logger().info("Model Loader node successfully created")
def __init__(self): super().__init__('velocity_publisher') self.get_logger().info( 'Initializing Co-operative shuttlebus controller') # self.group = ReentrantCallbackGroup() # to allow callbacks to be executed in parallel self.group = MutuallyExclusiveCallbackGroup() self.use_sim_time = self.get_parameter( 'use_sim_time').get_parameter_value().bool_value self.get_logger().info("use_sim_time: " + str(self.use_sim_time)) self.ROBOTNAMESPACE = os.environ['ROBOTNAMESPACE'] self.rviz = True self.Log_DATA = True # size of robots as a radius of minimum spanning disk # https://emanual.robotis.com/docs/en/platform/turtlebot3/specifications/ # the wheels on the robot is placed in the front. The robot is 138mm long and 178mm wide, thus asssuming the wheel to be placed in the front an conservative estimate is # from https://www.robotis.us/turtlebot-3-burger-us/ a more appropriate value would be 105 mm self.radius = 0.138 self.z_meas = torch.tensor([0, 0, 0], dtype=torch.float) self.deltaT = torch.tensor([1.0], dtype=torch.float) # in seconds self.Tau_span = 4 # prediction horizon self.svi_epochs = 10 if self.use_sim_time: self.covariance_scale = torch.tensor([1.0], dtype=torch.float) else: self.covariance_scale = torch.tensor( [0.1], dtype=torch.float ) # the covariances from AMCL seems very conservative - probably due to a need for population error in the particle filter. thus we scale it to be less conservative self.r_plan = 0.2 if self.ROBOTNAMESPACE == "turtlebot1": self.z_plan = torch.tensor([[2.0000, 1.0000, 0.0000, self.r_plan], [0.0000, 1.0000, 0.0000, self.r_plan]], dtype=torch.float) elif self.ROBOTNAMESPACE == "turtlebot2": self.z_plan = torch.tensor([[1.5000, 1.8660, 0.0000, self.r_plan], [0.5000, 0.1340, 0.0000, self.r_plan]], dtype=torch.float) elif self.ROBOTNAMESPACE == "turtlebot3": self.z_plan = torch.tensor([[0.5000, 1.8660, 0.0000, self.r_plan], [1.5000, 0.1340, 0.0000, self.r_plan]], dtype=torch.float) else: self.z_plan = torch.tensor( [[2.99, 2.57, 0, self.r_plan], [2.01, 1.93, 0, self.r_plan], [1.65, 1.12, 0, self.r_plan], [1.25, 0.65, 0, self.r_plan], [0.039, -0.18, 0, self.r_plan], [1.25, 0.65, 0, self.r_plan], [1.65, 1.12, 0, self.r_plan], [2.01, 1.93, 0, self.r_plan]], dtype=torch.float) self.i_plan = 0 self.meas_L = torch.diag( torch.tensor([1., 1., 1.], dtype=torch.float) ) # cholesky decomposition of measurement model covariance - this is just a temporary assignment it is reassigned in the meas callback # setup the inference algorithm optim_args = {"lr": 0.05} optimizer = pyro.optim.Adam(optim_args) self.svi = pyro.infer.SVI(model=decisionModel, guide=decisionGuide, optim=optimizer, loss=pyro.infer.Trace_ELBO(num_particles=1)) self.get_logger().info('Finished SVI setup') self.est_planning_epoch_time = Duration( nanoseconds=(self.deltaT[0] / 3.) * 10** 9) #variable to save Cumulative moving average of execution times self.N_samples_epoch_time = 1 self.a_current = torch.tensor([0.0, 0.0], dtype=torch.float) self.a_next = torch.tensor( [0.0, 0.0], dtype=torch.float ) # variable to save the current estimate of next action if not self.use_sim_time: # to get current pose from tf tree self.tfbuffer = tf2_ros.Buffer() self.tflistener = tf2_ros.TransformListener(self.tfbuffer, self) if self.use_sim_time: self.poseSubscription = self.create_subscription( Odometry, 'odom', self.pose_callback, 1, callback_group=self.group) self.poseSubscription # prevent unused variable warning else: self.poseSubscription = self.create_subscription( PoseWithCovarianceStamped, 'amcl_pose', self.pose_callback, 1, callback_group=self.group) self.poseSubscription # prevent unused variable warning self.subscriberManager_timer = self.create_timer( 2, self.subscriberManager_callback, callback_group=self.group) self.plannedActions_subscriptions = {} self.knownPlannedPaths = {} self.get_logger().info('Created subscriptions') self.plannedActionsPublisher_ = self.create_publisher( PlannedActions, 'plannedActions', 1, callback_group=self.group) if self.rviz == True: self.plannedPathPublisher_rviz = {} for tau in range(self.Tau_span): #self.plannedPathPublisher_rviz[tau] = self.create_publisher(PoseWithCovarianceStamped, 'Path_rviz_'+str(tau), 1, callback_group=self.group) self.plannedPathPublisher_rviz[tau] = self.create_publisher( PointCloud, 'Path_PointCloud_' + str(tau), 1, callback_group=self.group) self.goalPublisher_ = self.create_publisher(PointStamped, 'currentGoal', 1, callback_group=self.group) goalMsg = PointStamped() if self.use_sim_time: goalMsg.header.frame_id = "odom" else: goalMsg.header.frame_id = "map" goalMsg.header.stamp = self.get_clock().now().to_msg() goalMsg.point.x = self.z_plan[self.i_plan][0].item() goalMsg.point.y = self.z_plan[self.i_plan][1].item() goalMsg.point.z = 0.0 self.goalPublisher_.publish(goalMsg) self.velPublisher_ = self.create_publisher(Twist, 'publishedVel', 1) self.get_logger().info('Created publishers') planning_timer_period = self.deltaT.item() # seconds self.planning_timer = self.create_timer(planning_timer_period, self.planning_callback, callback_group=self.group) # align planning_timer for all robots self.t0 = alignTimer(self.get_clock(), self.planning_timer) self.tau, mod = divmod(self.t0.nanoseconds, int(self.deltaT.item() * (10**9))) if mod > self.deltaT.item() * (10**9) / 2: # divmod rounds down self.tau = self.tau + 1 self.get_logger().info('Controller Initialized') self.get_logger().info('z_0' + str(self.z_meas)) if self.Log_DATA: outdir = '/root/DATA/logs/' + self.ROBOTNAMESPACE if not os.path.exists(outdir): os.mkdir(outdir) filename = self.ROBOTNAMESPACE + "-" + datetime.now().strftime( "%d_%m_%Y-%H_%M_%S") + ".pkl" logFilePath = os.path.join(outdir, filename) self.logFileObject = open(logFilePath, 'ab') self.get_logger().info('Data Logging Initialized')
def __init__(self): super().__init__(MutuallyExclusiveCallbackGroup())
def __init__(self): """Create a WebServerNode and launch a flask server with default host and port details. """ super().__init__("webserver_publisher_node") self.get_logger().info("webserver_publisher_node started") HOST_DEFAULT = "0.0.0.0" PORT_DEFAULT = "5001" self.get_logger().info(f"Running the flask server on {HOST_DEFAULT}:{PORT_DEFAULT}") # Run the Flask webserver as a background thread. self.get_logger().info("Running webserver") self.server_thread = threading.Thread(target=app.run, daemon=True, kwargs={ "host": HOST_DEFAULT, "port": PORT_DEFAULT, "use_reloader": False, "threaded": True} ) self.server_thread.start() # Create service clients. # Create a reentrant callback group to set the vehicle mode. vehicle_mode_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create vehicle state service client: {VEHICLE_STATE_SERVICE}") self.vehicle_state_cli = self.create_client(ActiveStateSrv, VEHICLE_STATE_SERVICE, callback_group=vehicle_mode_cb_group) self.wait_for_service_availability(self.vehicle_state_cli) # Create a reentrant callback group to activate the state. enable_state_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create enable state service client: {ENABLE_STATE_SERVICE}") self.enable_state_cli = self.create_client(EnableStateSrv, ENABLE_STATE_SERVICE, callback_group=enable_state_cb_group) self.wait_for_service_availability(self.enable_state_cli) # Create a reentrant callback group to get the calibration value. get_calibration_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get_cal service client: {GET_CAR_CAL_SERVICE}") self.get_cal_cli = self.create_client(GetCalibrationSrv, GET_CAR_CAL_SERVICE, callback_group=get_calibration_cb_group) self.wait_for_service_availability(self.get_cal_cli) # Create a mutually exclusive callback group to set the calibration value. set_calibration_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set_cal service client: {SET_CAR_CAL_SERVICE}") self.set_cal_cli = self.create_client(SetCalibrationSrv, SET_CAR_CAL_SERVICE, callback_group=set_calibration_cb_group) self.wait_for_service_availability(self.set_cal_cli) # Create a reentrant callback group to get the device info values. get_device_info_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create device info service client: {GET_DEVICE_INFO_SERVICE}") self.get_revision_info_cli = self.create_client(GetDeviceInfoSrv, GET_DEVICE_INFO_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.get_revision_info_cli) self.get_logger().info(f"Create battery level service client: {BATTERY_LEVEL_SERVICE}") self.battery_level_cli = self.create_client(BatteryLevelSrv, BATTERY_LEVEL_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.battery_level_cli) self.get_logger().info(f"Create sensor status service client: {SENSOR_DATA_STATUS_SERVICE}") self.sensor_status_cli = self.create_client(SensorStatusCheckSrv, SENSOR_DATA_STATUS_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.sensor_status_cli) # Create a mutually exclusive callback group to set the tail light LED color values. set_led_color_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set led color service client: {SET_CAR_LED_SERVICE}") self.set_led_color_cli = self.create_client(SetLedCtrlSrv, SET_CAR_LED_SERVICE, callback_group=set_led_color_cb_group) self.wait_for_service_availability(self.set_led_color_cli) # Create a reentrant callback group to set the tail light LED color values. get_led_color_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get led color service client: {GET_CAR_LED_SERVICE}") self.get_led_color_cli = self.create_client(GetLedCtrlSrv, GET_CAR_LED_SERVICE, callback_group=get_led_color_cb_group) self.wait_for_service_availability(self.get_led_color_cli) # Create a reentrant callback group to call load model services. model_load_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create verify model ready service client: {VERIFY_MODEL_READY_SERVICE}") self.verify_model_ready_cli = self.create_client(VerifyModelReadySrv, VERIFY_MODEL_READY_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.verify_model_ready_cli) self.get_logger().info(f"Create configure LiDAR service client: {CONFIGURE_LIDAR_SERVICE}") self.configure_lidar_cli = self.create_client(LidarConfigSrv, CONFIGURE_LIDAR_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.configure_lidar_cli) self.get_logger().info(f"Create model state service client: {MODEL_STATE_SERVICE}") self.model_state_cli = self.create_client(ModelStateSrv, MODEL_STATE_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.model_state_cli) # Create a reentrant callback group to call model loading status service. is_model_loading_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create is model loading service client: {IS_MODEL_LOADING_SERVICE}") self.is_model_loading_cli = self.create_client(GetModelLoadingStatusSrv, IS_MODEL_LOADING_SERVICE, callback_group=is_model_loading_cb_group) self.wait_for_service_availability(self.is_model_loading_cli) # Create a mutually exclusive callback group to call model action services. model_action_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create upload model service client: {CONSOLE_MODEL_ACTION_SERVICE}") self.model_action_cli = self.create_client(ConsoleModelActionSrv, CONSOLE_MODEL_ACTION_SERVICE, callback_group=model_action_cb_group) self.wait_for_service_availability(self.model_action_cli) # Create a reentrant callback group to call software update check service. sw_update_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update state check service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_state_cli = self.create_client(SoftwareUpdateCheckSrv, SOFTWARE_UPDATE_CHECK_SERVICE_NAME, callback_group=sw_update_state_cb_group) self.wait_for_service_availability(self.sw_update_state_cli) # Create a reentrant callback group to call software update services. sw_update_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create begin sw update service client: {BEGIN_UPDATE_SERVICE}") self.begin_sw_update_cli = self.create_client(BeginSoftwareUpdateSrv, BEGIN_UPDATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.begin_sw_update_cli) self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_status_cli = self.create_client(SoftwareUpdateStateSrv, SOFTWARE_UPDATE_STATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.sw_update_status_cli) # Create a reentrant exclusive callback group for the software progress subscriber. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.sw_update_pct_sub_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_STATE_SERVICE}") self.sw_update_pct_sub = self.create_subscription(SoftwareUpdatePctMsg, SOFTWARE_UPDATE_PCT_TOPIC, self.sw_update_pct_sub_cb, callback_group=self.sw_update_pct_sub_cb_group, qos_profile=qos_profile) # Create a reentrant callback group to call autonomous throttle update service. set_throttle_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create set throttle service client: " f"{AUTONOMOUS_THROTTLE_SERVICE}") self.set_throttle_cli = self.create_client(NavThrottleSrv, AUTONOMOUS_THROTTLE_SERVICE, callback_group=set_throttle_cb_group) self.wait_for_service_availability(self.set_throttle_cli) # Create a reentrant callback group to call Get Ctrl Modes service. get_ctrl_modes_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create Get Ctrl Modes service client: " f"{GET_CTRL_MODES_SERVICE}") self.get_ctrl_modes_cli = self.create_client(GetCtrlModesSrv, GET_CTRL_MODES_SERVICE, callback_group=get_ctrl_modes_cb_group) self.wait_for_service_availability(self.get_ctrl_modes_cli) # Create a reentrant callback group to call otg link state service. otg_link_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create otg link state service client: " f"{GET_OTG_LINK_STATE_SERVICE}") self.otg_link_state_cli = self.create_client(OTGLinkStateSrv, GET_OTG_LINK_STATE_SERVICE, callback_group=otg_link_state_cb_group) self.wait_for_service_availability(self.otg_link_state_cli) # Create a reentrant callback group to publish manual drive messages. manual_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create manual drive publisher: {MANUAL_DRIVE_TOPIC}") self.pub_manual_drive = self.create_publisher(ServoCtrlMsg, MANUAL_DRIVE_TOPIC, 1, callback_group=manual_pub_drive_msg_cb_group) # Create a reentrant callback group to publish calibration drive messages. cal_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create calibration drive publisher: {CAL_DRIVE_TOPIC}") self.pub_calibration_drive = self.create_publisher(ServoCtrlMsg, CAL_DRIVE_TOPIC, 1, callback_group=cal_pub_drive_msg_cb_group) # Double buffer object to safely updat the latest status and installation percentage # during software update setup. self.pct_dict_db = DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({"status": "unknown", "update_pct": 0.0}) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback)
def __init__( self, node_name: str, *, context: Context = None, cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, start_parameter_services: bool = True, initial_parameters: List[Parameter] = None, allow_undeclared_parameters: bool = False, automatically_declare_initial_parameters: bool = True) -> None: """ Constructor. :param node_name: A name to give to this node. Validated by :func:`validate_node_name`. :param context: The context to be associated with, or ``None`` for the default global context. :param cli_args: A list of strings of command line args to be used only by this node. :param namespace: The namespace to which relative topic and service names will be prefixed. Validated by :func:`validate_namespace`. :param use_global_arguments: ``False`` if the node should ignore process-wide command line args. :param start_parameter_services: ``False`` if the node should not create parameter services. :param initial_parameters: A list of parameters to be set during node creation. :param allow_undeclared_parameters: True if undeclared parameters are allowed. This flag affects the behavior of parameter-related operations. :param automatically_declare_initial_parameters: True if initial parameters have to be declared upon node creation, false otherwise. """ self.__handle = None self._context = get_default_context() if context is None else context self._parameters: dict = {} self.__publishers: List[Publisher] = [] self.__subscriptions: List[Subscription] = [] self.__clients: List[Client] = [] self.__services: List[Service] = [] self.__timers: List[WallTimer] = [] self.__guards: List[GuardCondition] = [] self.__waitables: List[Waitable] = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None self._allow_undeclared_parameters = allow_undeclared_parameters self._initial_parameters = {} self._descriptors = {} namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self.__handle = Handle( _rclpy.rclpy_create_node(node_name, namespace, self._context.handle, cli_args, use_global_arguments)) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') with self.handle as capsule: self._logger = get_logger( _rclpy.rclpy_get_node_logger_name(capsule)) # Clock that has support for ROS time. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events) with self.handle as capsule: self._initial_parameters = _rclpy.rclpy_get_node_parameters( Parameter, capsule) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if initial_parameters is not None: self._initial_parameters.update( {p.name: p for p in initial_parameters}) if automatically_declare_initial_parameters: self._parameters.update(self._initial_parameters) self._descriptors.update( {p: ParameterDescriptor() for p in self._parameters}) if start_parameter_services: self._parameter_service = ParameterService(self)
def __init__(self): """Create a NetworkMonitorNode. """ super().__init__("network_monitor_node") self.get_logger().info("network_monitor_node started") self.state_dir = "" # Threading Event object to stop calling status LED update until the WiFi configuration # update is completed. self.led_update_pause = threading.Event() # Scheduler to queue the function calls and run them in a separate thread. self.scheduler = scheduler.Scheduler(self.get_logger()) # Timer to periodically update the WIFI_LED light with right color. if network_config.ENABLE_NETWORK_LED_UPDATE: self.update_status_LED_timer = self.create_timer( network_config.NETWORK_UPDATE_LED_PERIOD_IN_SECONDS, self.schedule_update_status_LED) # Timer to periodically attempt to write the connection status report to USB. if network_config.ENABLE_REPORT_STATE_UPDATE: self.report_state_timer = self.create_timer( network_config.REPORT_STATE_PERIOD_IN_SECONDS, self.schedule_report_state) # Publisher that broadcasts network connection status. self.network_status_pub_cb = ReentrantCallbackGroup() self.network_status_message_publisher = \ self.create_publisher(NetworkConnectionStatus, network_config.NETWORK_CONNECTION_STATUS_TOPIC_NAME, 1, callback_group=self.network_status_pub_cb) # Timer to periodically publish the network connection status. self.status_publisher_timer = self.create_timer(network_config.NETWORK_CONNECTION_PUBLISHER_PERIOD_IN_SECONDS, self.publish_network_connection_status, callback_group=self.network_status_pub_cb) # Clients to Status LED services that are called to indicate progress/success/failure # status while loading model. self.led_cb_group = MutuallyExclusiveCallbackGroup() self.led_blink_service = self.create_client(SetStatusLedBlinkSrv, constants.LED_BLINK_SERVICE_NAME, callback_group=self.led_cb_group) self.led_solid_service = self.create_client(SetStatusLedSolidSrv, constants.LED_SOLID_SERVICE_NAME, callback_group=self.led_cb_group) while not self.led_blink_service.wait_for_service(timeout_sec=1.0): self.get_logger().info("Led blink service not available, waiting again...") while not self.led_solid_service.wait_for_service(timeout_sec=1.0): self.get_logger().info("Led solid service not available, waiting again...") self.led_blink_request = SetStatusLedBlinkSrv.Request() self.led_solid_request = SetStatusLedSolidSrv.Request() # Client to USB File system subscription service that allows the node to connect to WiFi # based on the contents in the WiFi configuration file in the USB. The usb_monitor_node # will trigger notification if it finds the WiFi configuration file from the watchlist # in the USB drive. self.usb_sub_cb_group = ReentrantCallbackGroup() self.usb_file_system_subscribe_client = self.create_client(USBFileSystemSubscribeSrv, constants.USB_FILE_SYSTEM_SUBSCRIBE_SERVICE_NAME, callback_group=self.usb_sub_cb_group) while not self.usb_file_system_subscribe_client.wait_for_service(timeout_sec=1.0): self.get_logger().info("File System Subscribe not available, waiting again...") # Client to USB Mount point manager service to indicate that the usb_monitor_node can safely # decrement the counter for the mount point once the action function for the WiFi configuration # file being watched by network_monitor_node is succesfully executed. self.usb_mpm_cb_group = ReentrantCallbackGroup() self.usb_mount_point_manager_client = self.create_client(USBMountPointManagerSrv, constants.USB_MOUNT_POINT_MANAGER_SERVICE_NAME, callback_group=self.usb_mpm_cb_group) while not self.usb_mount_point_manager_client.wait_for_service(timeout_sec=1.0): self.get_logger().info("USB mount point manager service not available, waiting again...") # Subscriber to USB File system notification publisher to recieve the broadcasted messages # with file/folder details, whenever a watched file is identified in the USB connected. self.usb_notif_cb_group = ReentrantCallbackGroup() self.usb_file_system_notification_sub = self.create_subscription(USBFileSystemNotificationMsg, constants.USB_FILE_SYSTEM_NOTIFICATION_TOPIC, self.usb_file_system_notification_cb, 10, callback_group=self.usb_notif_cb_group) # Add the "wifi-creds.txt" file to the watchlist. usb_file_system_subscribe_request = USBFileSystemSubscribeSrv.Request() usb_file_system_subscribe_request.file_name = network_config.WIFI_CONFIG_NAME usb_file_system_subscribe_request.callback_name = network_config.SCHEDULE_CONFIG_UPDATE_CB usb_file_system_subscribe_request.verify_name_exists = True self.usb_file_system_subscribe_client.call_async(usb_file_system_subscribe_request) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback) self.get_logger().info("Network Monitor node successfully created")
def __init__(self): """Create a SoftwareUpdateNode. """ super().__init__("software_update_process") self.get_logger().info("software_update_process started") # Scheduler to queue the function calls and run them in a separate thread. self.scheduler = scheduler.Scheduler(self.get_logger()) # Threading lock object to safely update the update_state variable. self.state_guard = threading.Lock() self.update_state = software_update_config.SoftwareUpdateState.UPDATE_UNKNOWN # Threading Event object to indicate if the software update check is completed. self.check_complete = threading.Event() # Flag to indicate software update check in progress. self.check_in_progress = False # List of packages that have updates available. self.update_list = list() # Flag to identify if the network is connected. self.is_network_connected = False # Flag to identify if software update check has to performed # again when network is connected. self.reschedule_software_update_check = False # The apt cache object. self.cache = apt.Cache() # Double buffer object containing the current update percentage and progress state. self.pct_dict_db = utility.DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({ software_update_config.PROG_STATE_KEY: software_update_config.PROGRESS_STATES[0], software_update_config.PROG_PCT_KEY: 0.0 }) # Timer to periodically check for software update. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.get_logger().info( "Schedule the software update check every " f"{software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS} " "seconds.") self.schedule_update_check_cb = ReentrantCallbackGroup() self.update_check_timer = \ self.create_timer(software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS, self.schedule_update_check, callback_group=self.schedule_update_check_cb) # Publisher that sends software update pct and status. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.pct_obj = SoftwareUpdatePctMsg() self.software_update_pct_pub_cb = ReentrantCallbackGroup() self.software_update_pct_publisher = \ self.create_publisher(SoftwareUpdatePctMsg, software_update_config.SOFTWARE_UPDATE_PCT_TOPIC_NAME, qos_profile=qos_profile, callback_group=self.software_update_pct_pub_cb) # Service to check if there is a software update available. self.software_update_check_cb_group = ReentrantCallbackGroup() self.software_update_check_service = \ self.create_service(SoftwareUpdateCheckSrv, software_update_config.SOFTWARE_UPDATE_CHECK_SERVICE_NAME, self.software_update_check, callback_group=self.software_update_check_cb_group) # Service to execute the software update and install latest packages. self.begin_update_service_cb_group = ReentrantCallbackGroup() self.begin_update_service = \ self.create_service(BeginSoftwareUpdateSrv, software_update_config.BEGIN_SOFTWARE_UPDATE_SERVICE_NAME, self.begin_update, callback_group=self.begin_update_service_cb_group) # Service to get the current software update state. self.software_update_state_service_cb_group = ReentrantCallbackGroup() self.software_update_state_service = \ self.create_service(SoftwareUpdateStateSrv, software_update_config.SOFTWARE_UPDATE_STATE_SERVICE_NAME, self.software_update_state, callback_group=self.software_update_state_service_cb_group) self.nw_con_status_cb_group = ReentrantCallbackGroup() self.network_connection_status_sub = \ self.create_subscription(NetworkConnectionStatus, software_update_config.NETWORK_CONNECTION_STATUS_TOPIC_NAME, self.network_connection_status_cb, 10, callback_group=self.nw_con_status_cb_group) # Clients to Status LED services that are called to indicate progress/success/failure # status while loading model. self.led_cb_group = MutuallyExclusiveCallbackGroup() self.led_blink_service = self.create_client( SetStatusLedBlinkSrv, constants.LED_BLINK_SERVICE_NAME, callback_group=self.led_cb_group) self.led_solid_service = self.create_client( SetStatusLedSolidSrv, constants.LED_SOLID_SERVICE_NAME, callback_group=self.led_cb_group) while not self.led_blink_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led blink service not available, waiting again...") while not self.led_solid_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led solid service not available, waiting again...") self.led_blink_request = SetStatusLedBlinkSrv.Request() self.led_solid_request = SetStatusLedSolidSrv.Request() # Client to USB File system subscription service that allows the node to install # signed software packages(*.deb.gpg) present in the USB. The usb_monitor_node # will trigger notification if it finds the "update" folder from the watchlist # in the USB drive. self.usb_sub_cb_group = ReentrantCallbackGroup() self.usb_file_system_subscribe_client = self.create_client( USBFileSystemSubscribeSrv, constants.USB_FILE_SYSTEM_SUBSCRIBE_SERVICE_NAME, callback_group=self.usb_sub_cb_group) while not self.usb_file_system_subscribe_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "File System Subscribe not available, waiting again...") # Client to USB Mount point manager service to indicate that the usb_monitor_node can safely # decrement the counter for the mount point once the action function for the "update" folder # file being watched by software_update_node is succesfully executed. self.usb_mpm_cb_group = ReentrantCallbackGroup() self.usb_mount_point_manager_client = self.create_client( USBMountPointManagerSrv, constants.USB_MOUNT_POINT_MANAGER_SERVICE_NAME, callback_group=self.usb_mpm_cb_group) while not self.usb_mount_point_manager_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "USB mount point manager service not available, waiting again..." ) # Subscriber to USB File system notification publisher to recieve the broadcasted messages # with file/folder details, whenever a watched file is identified in the USB connected. self.usb_notif_cb_group = ReentrantCallbackGroup() self.usb_file_system_notification_sub = self.create_subscription( USBFileSystemNotificationMsg, constants.USB_FILE_SYSTEM_NOTIFICATION_TOPIC, self.usb_file_system_notification_cb, 10, callback_group=self.usb_notif_cb_group) # Add the "update" folder to the watchlist. usb_file_system_subscribe_request = USBFileSystemSubscribeSrv.Request() usb_file_system_subscribe_request.file_name = software_update_config.UPDATE_SOURCE_DIRECTORY usb_file_system_subscribe_request.callback_name = software_update_config.SCHEDULE_USB_UPDATE_SCAN_CB usb_file_system_subscribe_request.verify_name_exists = True self.usb_file_system_subscribe_client.call_async( usb_file_system_subscribe_request) # Set the power led to blue. self.led_solid_request.led_index = constants.LEDIndex.POWER_LED self.led_solid_request.color = constants.LEDColor.BLUE self.led_solid_request.hold = 0.0 self.led_solid_service.call_async(self.led_solid_request) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback) # Schedule update check. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.schedule_update_check() self.get_logger().info("Software Update node successfully created")