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)
# ========== マルチスレッドテスト１ ==========
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")
# ========== マルチスレッドテスト２ ==========
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)
# ========== マルチスレッドテスト１ ==========
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)
# ========== マルチスレッドテスト２ ==========
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")