diff --git a/ros_acomms/src/mac_utils.py b/ros_acomms/src/mac_utils.py index b8c79735651b48d83e5684c399794aa87c7a6cb0..a46b12a20be76c50b9f92eb71bad89e1eb680a7e 100755 --- a/ros_acomms/src/mac_utils.py +++ b/ros_acomms/src/mac_utils.py @@ -18,7 +18,8 @@ KeyValue = armw.import_message("diagnostic_msgs", "KeyValue") FDPMaxBytes4Rate = namedtuple('FDPMaxBytes4Rate', 'rate max_bytes default packet_duration_ms') ManualTransmitRequest = namedtuple('ManualTransmitRequest', 'subscriber_queue publisher publish_topic subscribe_topic') ManagedMacNodeEntry = namedtuple('ManagedMacNodeEntry', 'ns is_default set_software_mute select_callback') -re_slot_range = r"^(?P<start>[0-9]*)(:|;|-)(?P<end>[0-9]*)(:|;|-)?((?P<step>[0-9]*))?$" +# re_slot_range = r"^(?P<start>[0-9]*)(:|;|-)(?P<end>[0-9]*)(:|;|-)?((?P<step>[0-9]*))?$" +re_slot_range = r"^(?P<start>-?[0-9]*)(:|;)(?P<end>-?[0-9]*)(:|;)?((?P<step>[0-9]*))?$" class PSK_ROS_FDP_Rates(object): FDP_MAX_BYTES_FOR_MINI_RATE = [ @@ -35,6 +36,12 @@ class PSK_ROS_FDP_Rates(object): None, None, None, None, None, None, None, ] +def resolve_slot_index(index, last_valid_slot, first_valid_slot=0): + if index >= 0: + return index + positive_indices = [i for i in range(first_valid_slot, last_valid_slot + 1)] + return positive_indices[index] + def validate_slot_range(slot_range, last_valid_slot, first_valid_slot=0, default_step=1): match = re.match(re_slot_range, slot_range) if match: @@ -42,17 +49,28 @@ def validate_slot_range(slot_range, last_valid_slot, first_valid_slot=0, default start, end, step = first_valid_slot, last_valid_slot, default_step # we matched on this pattern. fill empty groups with defaults if match.groupdict()['start']: - start = int(match.groupdict()['start']) + start = resolve_slot_index(int(match.groupdict()['start']), last_valid_slot, first_valid_slot) if match.groupdict()['end']: - end = int(match.groupdict()['end']) + end = resolve_slot_index(int(match.groupdict()['end']), last_valid_slot, first_valid_slot) if match.groupdict()['step']: step = int(match.groupdict()['step']) return [start, end, step] +def slot_range_to_list(slot_range, last_valid_slot, first_valid_slot=0, default_step=1): + slot_range = validate_slot_range(slot_range, last_valid_slot, first_valid_slot, default_step) + + if slot_range is not None: + start, end, step = slot_range[0], slot_range[1], slot_range[2] + # handle potential wrap around + if end <= start: + # range wraps around end of cycle to start + return list([i for i in range(0, (end + 1), step)] +\ + [i for i in range(start, (last_valid_slot + 1), step)]) + return [i for i in range(start, (end + 1), step)] + def format_range(start, end, step): - return '{}-{}:{}'.format(start, end, step) - # return '{}-{}:{}'.format(start, end + step, step) + return '{}:{}:{}'.format(start, end, step) def parse_range(lst): if len(lst) == 1: diff --git a/ros_acomms/src/message_queue_node.py b/ros_acomms/src/message_queue_node.py index df52478fb14d3ae295489a6db433875b19185642..5823218617f9ec0420ff15f3c522f2f98ae862aa 100755 --- a/ros_acomms/src/message_queue_node.py +++ b/ros_acomms/src/message_queue_node.py @@ -537,7 +537,9 @@ class MessageQueueNode(armw.ArmwNode): # this method handles finding the highest priority packet codec, then using the packet codec type to encode the packet returned. # TODO: Need to validate that selected codec can fit with overhead # TODO: Support multiple packet_codecs in one packet - response, messages_in_packet = self.get_next_packet(req=req) + # clear the current_packet_request before starting next round + self.current_packet_request = req + response, messages_in_packet = self.get_next_packet() # planning to do more here with status on outbound traffic # .. return response @@ -548,7 +550,10 @@ class MessageQueueNode(armw.ArmwNode): # we ONLY call release if WE have the lock. Once enter try/catch he can be sure we have the lock. self.get_next_packet_data_lock.release() - def get_next_packet(self, req: GetNextPacketDataRequest): + def get_next_packet(self, req: GetNextPacketDataRequest = None): + # use current_packet_request which will be used for filling the rest of the packet during calls from the packet_codec + if req is None: + req = self.current_packet_request # assumes req.minimum_priority is linted by caller (handle_get_next_packet) # this is the default value. Tags may change the minimum_priority in the future minimum_priority = req.minimum_priority @@ -608,7 +613,7 @@ class MessageQueueNode(armw.ArmwNode): self.mark_transmitted(msg) except: - self.log_err(f"Exception in get_next_packet_data() \nTraceback:\n{traceback.format_exc()}") + self.log_error(f"Exception in get_next_packet_data() \nTraceback:\n{traceback.format_exc()}") return GetNextPacketDataResponse(), messages_in_packet else: # handle_get_next_packet was a success! We are returning an encoded payload @@ -622,11 +627,15 @@ class MessageQueueNode(armw.ArmwNode): def get_highest_priority_codec(self, available_packet_bytes: int, dest_address: Optional[int] = None, - minimum_priority: int = None): + minimum_priority: int = None, + req: GetNextPacketDataRequest = None,): + if req is None: + req = self.current_packet_request highest_priority_message = self.get_next_message(max_size_in_bits=available_packet_bytes*8, include_packet_overhead_in_size=True, dest_address=dest_address, minimum_priority=minimum_priority, + req=req, ) if highest_priority_message: @@ -637,14 +646,18 @@ class MessageQueueNode(armw.ArmwNode): return packet_codec, dest_address - def get_next_message(self, max_size_in_bits: int = 131072, + def get_next_message(self, + req: GetNextPacketDataRequest = None, + max_size_in_bits: int = 131072, include_packet_overhead_in_size = False, dest_address: Optional[int] = None, packet_codec: Optional[str] = None, check_reachability: bool = True, minimum_priority: int = None, - exclude_messages: Optional[List[QueuedMessage]] = []) -> Optional[QueuedMessage]: + exclude_messages: Optional[List[QueuedMessage]] = [],) -> Optional[QueuedMessage]: + if req is None: + req = self.current_packet_request # We need these "link_layer" tags to pass to dynamic queue endpoints. # These other nodes may have nested tags and exclusion criteria, this is how we tell those nodes about, # .. the link_layer tags. Now nodes using the dynamic queue service can customize their behavior @@ -669,6 +682,7 @@ class MessageQueueNode(armw.ArmwNode): exclude_messages=exclude_messages, link_layer_require_tags=link_layer_require_tags, link_layer_exclude_tags=link_layer_exclude_tags, + req=req, ) except: self.log_error(f"get_next_message Error querying dynamic queues:\n{traceback.format_exc()}") @@ -743,6 +757,7 @@ class MessageQueueNode(armw.ArmwNode): return True def query_dynamic_queues(self, + req: GetNextPacketDataRequest = None, max_size_in_bits: int = 131072, include_packet_overhead_in_size = False, dest_address: Optional[int] = None, @@ -751,7 +766,7 @@ class MessageQueueNode(armw.ArmwNode): minimum_priority: int = None, exclude_messages: Optional[List[QueuedMessage]] = None, link_layer_require_tags=None, - link_layer_exclude_tags=None) -> tuple[Optional['DynamicQueue'], Optional[int]]: + link_layer_exclude_tags=None,) -> tuple[Optional['DynamicQueue'], Optional[int]]: """ Query the registered dynamic queues using the specified message filter criteria. Returns the highest priority dynamic queue matching those criteria. @@ -775,6 +790,9 @@ class MessageQueueNode(armw.ArmwNode): the dynamic queues, returns (None, None). """ + if req is None: + req = self.current_packet_request + if link_layer_exclude_tags is None: link_layer_exclude_tags = [] if link_layer_require_tags is None: @@ -856,7 +874,12 @@ class MessageQueueNode(armw.ArmwNode): str(packet_codec) if packet_codec else ''), exclude_message_ids=this_queue_exclusion_ids, require_tags=link_layer_require_tags, - exclude_tags=link_layer_exclude_tags) + exclude_tags=link_layer_exclude_tags, + mac_namespace=req.mac_namespace, + mac_cycle_num=req.mac_cycle_num, + slot_in_cycle_num=req.slot_in_cycle_num, + packet_in_slot_num=req.packet_in_slot_num, + ) self.log_debug(f"Queried {dynamic_queue_name} for {message_max_size_in_bits} bits, got: {queue_response}") except Exception as e: self.log_error_throttle(1, f"Error calling get_next_message service on {dynamic_queue_name}: {e}") diff --git a/ros_acomms/src/tdma_advanced_node.py b/ros_acomms/src/tdma_advanced_node.py index 8b016f7948eda64fea58eded3abff9ded413deab..5ee01f3ededbdeb5586a23ae59d1632a333865d0 100755 --- a/ros_acomms/src/tdma_advanced_node.py +++ b/ros_acomms/src/tdma_advanced_node.py @@ -389,7 +389,7 @@ class TdmaAdvancedMacNode(TdmaMacNode): update_last_tx = True return sent_modem_pings, sent_transponder_ping, update_last_tx - def handle_queuing_packet(self, current_slot, remaining_active_seconds, sent_modem_pings, sent_transponder_ping): + def handle_queuing_packet(self, current_slot, remaining_active_seconds, sent_modem_pings, sent_transponder_ping, mac_namespace=None): sent_modem_pings, sent_transponder_ping, update_last_tx = self.handle_this_cycle_nav_pings(current_slot, remaining_active_seconds, sent_modem_pings, sent_transponder_ping) if update_last_tx: # sent nav pings, return @@ -398,14 +398,10 @@ class TdmaAdvancedMacNode(TdmaMacNode): if current_slot in self.comms_slots: # check and make sure comms are allowed this slot # no nav pings sent this iter, comms allowed, go ahead with packet - position_in_queue = self.send_next_packet() + position_in_queue = self.send_next_packet(mac_namespace=mac_namespace if mac_namespace is not None else self.get_name()) if position_in_queue is not None: update_last_tx = True - # after sending comms packet, we clear the flags on nav pings - # after sending all modem pings requested, a transponder ping (if requested) is sent out, - # .. then comms are allowed to use the remaining active time - sent_modem_pings, sent_transponder_ping = False, False self.log_debug(f'DEBUG: tdma advanced sent packet, position in queue: {position_in_queue}') return sent_modem_pings, sent_transponder_ping, update_last_tx diff --git a/ros_acomms/src/tdma_node.py b/ros_acomms/src/tdma_node.py index 35529a7b71974123a5df5ee2f63187fa6c9386cc..e2462bab4614ca36f53039a5220c39fb76d0590a 100755 --- a/ros_acomms/src/tdma_node.py +++ b/ros_acomms/src/tdma_node.py @@ -19,6 +19,7 @@ QueueTxPacket, _, QueueTxPacketRequest = armw.import_service("ros_acomms_msgs", GetNextPacketData, _, _ = armw.import_service("ros_acomms_msgs", "GetNextPacketData") Packet = armw.import_message("ros_acomms_msgs", "Packet") TdmaStatus = armw.import_message("ros_acomms_msgs", "TdmaStatus") +MacStatus = armw.import_message("ros_acomms_msgs", "MacStatus") class TdmaMacNode(armw.ArmwNode): def __init__(self, subclass=False): @@ -26,6 +27,13 @@ class TdmaMacNode(armw.ArmwNode): if not subclass: self.tdma_status_publisher = self.publish('~tdma_status' if self.get_param('~publish_private_status', False) else 'tdma_status', TdmaStatus, queue_size=3) + + # For MAC introspection + self.mac_cycle_num = 0 + self.slot_in_cycle_num = 0 + self.packet_in_slot_num = 0 + + self.mac_status_publisher = self.publish('mac_status', MacStatus, queue_size=3) self.setup_active_slots( active_slots=self.get_param('~active_slots', 0), # active_slots: (int)/[int,int,..] turns this param into list of ints, slots we're active in @@ -409,7 +417,17 @@ class TdmaMacNode(armw.ArmwNode): while not armw.is_shutdown(): rate.sleep() + # increase on each slot change + self.slot_in_cycle_num += 1 + # reset on each slot change + self.packet_in_slot_num = 0 + current_slot, remaining_slot_seconds = self.get_current_slot() + + if current_slot == 0: + # increase on each complete cycle + self.mac_cycle_num += 1 + if last_slot is None: last_slot = current_slot @@ -440,7 +458,7 @@ class TdmaMacNode(armw.ArmwNode): # not enough time. sleep for rate and try again return - def send_next_packet(self, insert_at_head=False, minimum_priority=None, slot_tags=None): + def send_next_packet(self, insert_at_head=False, minimum_priority=None, slot_tags=None, mac_namespace=None): self.log_debug("Entering send_next_packet") # this will not send a manual request when minimum_priority is passed # .. unless ~allow_manual_tx_during_priority_slots is set to True (defaults to False) @@ -465,7 +483,11 @@ class TdmaMacNode(armw.ArmwNode): num_dataframe_bytes=self.maximum_dataframe_bytes, minimum_priority=minimum_priority, require_tags=list(slot_tags[current_slot].require_tags), - exclude_tags=list(slot_tags[current_slot].exclude_tags)) + exclude_tags=list(slot_tags[current_slot].exclude_tags), + mac_namespace=mac_namespace if mac_namespace is not None else self.get_name(), + mac_cycle_num=self.mac_cycle_num, + slot_in_cycle_num=self.slot_in_cycle_num, + packet_in_slot_num=self.packet_in_slot_num) except Exception as e: self.log_warn(f'WARNING: send_next_packet() threw error on get_next_packet_data service callback!') return -1 @@ -505,6 +527,15 @@ class TdmaMacNode(armw.ArmwNode): packet=queue_packet) resp = self.queue_tx_packet(req) + if resp.position_in_queue is not None: + msg = MacStatus(mac_namespace=mac_namespace if mac_namespace is not None else self.get_name(), + mac_cycle_num=self.mac_cycle_num, + slot_in_cycle_num=self.slot_in_cycle_num, + packet_in_slot_num=self.packet_in_slot_num) + armw.fill_time(msg.header.stamp, armw.Time().now()) + self.mac_status_publisher.publish(msg) + # increase after each packet transmitted + self.packet_in_slot_num += 1 return resp.position_in_queue diff --git a/ros_acomms/src/tdma_scripted_node.py b/ros_acomms/src/tdma_scripted_node.py index 8ecccaa01128e2507e89997ed08597eb7f288ee4..3f45d0856d3d1543a86e45e8c3c1c43a01dd4b67 100755 --- a/ros_acomms/src/tdma_scripted_node.py +++ b/ros_acomms/src/tdma_scripted_node.py @@ -303,10 +303,11 @@ class TdmaScriptedMacNode(TdmaAdvancedMacNode): # we are active and have at least enough time to send another packet self.sent_modem_pings, self.sent_transponder_ping, update_last_tx = self.handle_queuing_packet( - self.msg.current_slot, - self.msg.remaining_active_seconds, - self.sent_modem_pings, - self.sent_transponder_ping + current_slot=self.msg.current_slot, + remaining_active_seconds=self.msg.remaining_active_seconds, + sent_modem_pings=self.sent_modem_pings, + sent_transponder_ping=self.sent_transponder_ping, + mac_namespace=self.get_name(), ) if update_last_tx: self.packet_duration_sec, self.scripted_msg = self.set_rate_max_bytes_for_next_tx() diff --git a/ros_acomms/src/tdma_slotted_aloha_node.py b/ros_acomms/src/tdma_slotted_aloha_node.py index 64f314169087cce2e17babe1f3653b543a7b5734..ce26c0b4cb5f4e254be9e5f9b6dc02829df24b44 100755 --- a/ros_acomms/src/tdma_slotted_aloha_node.py +++ b/ros_acomms/src/tdma_slotted_aloha_node.py @@ -203,9 +203,9 @@ class TdmaSlottedAlohaMacNode(TdmaAdvancedMacNode): position_in_queue = None # if aloha slot we pass the minimum_priority if self.in_aloha_slot(current_slot=current_slot): - position_in_queue = self.send_next_packet(minimum_priority=self.aloha_slot_priority) + position_in_queue = self.send_next_packet(minimum_priority=self.aloha_slot_priority, mac_namespace=self.get_name()) elif current_slot in self.comms_slots: - position_in_queue = self.send_next_packet() + position_in_queue = self.send_next_packet(mac_namespace=self.get_name()) if position_in_queue is not None: update_last_tx = True diff --git a/ros_acomms_msgs/CMakeLists.txt b/ros_acomms_msgs/CMakeLists.txt index 7cb960fcb4ddf9bd53841ae1ce9c31c5d665281b..b15c1c383df968b1d705ea8497f5fcc8399901f6 100644 --- a/ros_acomms_msgs/CMakeLists.txt +++ b/ros_acomms_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1") SST.msg TTA.msg LinkStatsFeedback.msg + MacStatus.msg MacSwitcherStatus.msg ManagedMacState.msg ModemAck.msg @@ -82,6 +83,7 @@ elseif ("$ENV{ROS_VERSION}" STREQUAL "2") "msg/SST.msg" "msg/TTA.msg" "msg/LinkStatsFeedback.msg" + "msg/MacStatus.msg" "msg/MacSwitcherStatus.msg" "msg/ManagedMacState.msg" "msg/ModemAck.msg" diff --git a/ros_acomms_msgs/msg/MacStatus.msg b/ros_acomms_msgs/msg/MacStatus.msg new file mode 100644 index 0000000000000000000000000000000000000000..a476145b8b57a07cbcde163681896e45dfb724f6 --- /dev/null +++ b/ros_acomms_msgs/msg/MacStatus.msg @@ -0,0 +1,10 @@ +std_msgs/Header header + +# if filled out, this is the MAC that is building a packet to be transmitted +string mac_namespace +# Monotonically increasing count for rolling past active_slot 0. +uint64 mac_cycle_num +# Monotonically increasing count that increases on each slot change. +uint64 slot_in_cycle_num +# Starting at 0 for first packet in slot. If the slot is long enough to have more than 1 packet, this will count up +uint16 packet_in_slot_num diff --git a/ros_acomms_msgs/srv/GetNextPacketData.srv b/ros_acomms_msgs/srv/GetNextPacketData.srv index 2cd319a1b5dce194a9ec780b6e55ed50dc0d54ff..337179a838901d743c3ae81e1a4b654a4c65b5db 100644 --- a/ros_acomms_msgs/srv/GetNextPacketData.srv +++ b/ros_acomms_msgs/srv/GetNextPacketData.srv @@ -9,6 +9,16 @@ int8 minimum_priority string[] require_tags string[] exclude_tags +# MAC Introspection for dynamic queues. Information about the current cycle and slot that dynamic queue clients might act on +# if filled out, this is the MAC that is building a packet to be transmitted +string mac_namespace +# Monotonically increasing count for rolling past active_slot 0. +uint64 mac_cycle_num +# Monotonically increasing count that increases on each slot change. +uint64 slot_in_cycle_num +# Starting at 0 for first packet in slot. If the slot is long enough to have more than 1 packet, this will count up +uint16 packet_in_slot_num + --- # Response int16 dest # Only 0-255 used by the modem, but allow negative values here diff --git a/ros_acomms_msgs/srv/GetNextQueuedMessage.srv b/ros_acomms_msgs/srv/GetNextQueuedMessage.srv index c2908c4e4b19f29ff6f8552b9a76ffba79aa412d..c5e6f8260ccafa95dca62e2a785a073815025328 100644 --- a/ros_acomms_msgs/srv/GetNextQueuedMessage.srv +++ b/ros_acomms_msgs/srv/GetNextQueuedMessage.srv @@ -30,6 +30,15 @@ string[] require_tags # The criteria for exclusion are up to the servicer, but it should do something reasonable. string[] exclude_tags +# MAC Introspection for dynamic queues. Information about the current cycle and slot that dynamic queue clients might act on +# if filled out, this is the MAC that is building a packet to be transmitted +string mac_namespace +# Monotonically increasing count for rolling past active_slot 0. +uint64 mac_cycle_num +# Monotonically increasing count that increases on each slot change. +uint64 slot_in_cycle_num +# Starting at 0 for first packet in slot. If the slot is long enough to have more than 1 packet, this will count up +uint16 packet_in_slot_num --- diff --git a/ros_acomms_tests/launch/test_link_layer_tags.launch b/ros_acomms_tests/launch/test_link_layer_tags.launch index b334a05ca028973940f6c21961706b94554363b8..0f96dc23cdbf84a0f75a3d5fe1a661e2422c94a0 100644 --- a/ros_acomms_tests/launch/test_link_layer_tags.launch +++ b/ros_acomms_tests/launch/test_link_layer_tags.launch @@ -37,7 +37,7 @@ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false" clear_params="true"> <param name="active_slots" value="0,1" type="str" if="$(eval arg('tdma_type') == 'tdma_slotted_aloha')" /> - <param name="active_slots" value="0-11" type="str" if="$(eval arg('tdma_type') != 'tdma_slotted_aloha')" /> + <param name="active_slots" value="0:11" type="str" if="$(eval arg('tdma_type') != 'tdma_slotted_aloha')" /> <rosparam> num_slots: 12 slot_duration_seconds: 10 @@ -48,25 +48,25 @@ pings_per_slot: 1 always_send_test_data: True aloha_slot_priority: 0 - aloha_slots: 4-11 + aloha_slots: '4:11' </rosparam> <rosparam> slot_tags: - - slots: 0-11 + - slots: '0:11' exclude_tags: - 'always_exclude' - - slots: 0-2 + - slots: '0:2' require_tags: - 'extra_special' - - slots: 0 + - slots: '0' minimum_priority: 50 exclude_tags: - 'low_priority' - - slots: 6-11 + - slots: '6:11' require_tags: - 'chat' - - slots: 3,4,5 + - slots: '3,4,5' require_tags: - 'some_aloha' </rosparam> @@ -104,7 +104,7 @@ <node name="tdma" pkg="ros_acomms" type="$(arg tdma_type)_node.py" output="screen" required="true" respawn="false" clear_params="true"> <param name="active_slots" value="2,3" type="str" if="$(eval arg('tdma_type') == 'tdma_slotted_aloha')" /> - <param name="active_slots" value="0-11" type="str" if="$(eval arg('tdma_type') != 'tdma_slotted_aloha')" /> + <param name="active_slots" value="0:11" type="str" if="$(eval arg('tdma_type') != 'tdma_slotted_aloha')" /> <rosparam> num_slots: 12 slot_duration_seconds: 10 @@ -115,7 +115,7 @@ pings_per_slot: 1 always_send_test_data: True aloha_slot_priority: 0 - aloha_slots: 4-11 + aloha_slots: '4:11' </rosparam> <param name="slot_tags" value="$(dirname)/test_slot_tags.yaml" /> diff --git a/ros_acomms_tests/launch/test_slot_tags.yaml b/ros_acomms_tests/launch/test_slot_tags.yaml index 76fafc366b3a5a35ecba393bae2ef6846f915f76..fe5437ac363d25afca6e495a75e4c2079f9f07a9 100644 --- a/ros_acomms_tests/launch/test_slot_tags.yaml +++ b/ros_acomms_tests/launch/test_slot_tags.yaml @@ -1,17 +1,17 @@ slot_tags: - - slots: 0-2 + - slots: '0:2' require_tags: - 'extra_special' - - slots: 0-11 + - slots: '0:11' exclude_tags: - 'always_exclude' - - slots: 2 + - slots: '2' minimum_priority: 50 exclude_tags: - 'low_priority' - - slots: 4-11 + - slots: '4:11' require_tags: - 'chat' - - slots: 4-6 + - slots: '4:6' require_tags: - 'some_aloha' diff --git a/ros_acomms_tests/src/test_tdma_extreme_configs.py b/ros_acomms_tests/src/test_tdma_extreme_configs.py index bc1e346be3bf768d923caaca9fdbef791b8b78f7..e82e92a729dee9386476445f0a9674dd508ce617 100755 --- a/ros_acomms_tests/src/test_tdma_extreme_configs.py +++ b/ros_acomms_tests/src/test_tdma_extreme_configs.py @@ -102,16 +102,16 @@ class TestTdmaExtremeConfigs: if self.tdma_type == 'tdma_slotted_aloha': # assert str(expanded_slots_as_ranges(ret.nav_slots)) == '12-96:4', f'ERROR: nav_slots not expected range: 12-96:4, aloha_slots should trim nav_slots' - assert 'nav_slots: 12-96:4' in ret.message, f'ERROR: nav_slots not expected range: 12-96:4, aloha_slots should trim nav_slots' - assert 'aloha_slots: 100-152:2,0-10:1,280-65534:1' in ret.message, f'ERROR: aloha_slots not expected range: 100-152:2,0-10:1,280-65534:1' + assert 'nav_slots: 12:96:4' in ret.message, f'ERROR: nav_slots not expected range: 12-96:4, aloha_slots should trim nav_slots' + assert 'aloha_slots: 100:152:2,0:10:1,280:65534:1' in ret.message, f'ERROR: aloha_slots not expected range: 100-152:2,0-10:1,280-65534:1' else: # assert str(expanded_slots_as_ranges(ret.nav_slots)) == '0-148:4', f'ERROR: nav_slots not expected range: 0-148:4' - assert 'nav_slots: 0-148:4' in ret.message, f'ERROR: nav_slots not expected range: 0-148:4' + assert 'nav_slots: 0:148:4' in ret.message, f'ERROR: nav_slots not expected range: 0-148:4' # assert str(expanded_slots_as_ranges(ret.comms_slots)) == '0-150:2', f'ERROR: comms_slots not expected range: 0-150:2' # assert str(expanded_slots_as_ranges(ret.active_slots)) == '0-150:2', f'ERROR: active_slots not expected range: 0-150:2' - assert 'comms_slots: 0-150:2' in ret.message, f'ERROR: comms_slots not expected range: 0-150:2' - assert 'active_slots: 0-150:2' in ret.message, f'ERROR: active_slots not expected range: 0-150:2' + assert 'comms_slots: 0:150:2' in ret.message, f'ERROR: comms_slots not expected range: 0-150:2' + assert 'active_slots: 0:150:2' in ret.message, f'ERROR: active_slots not expected range: 0-150:2' except rospy.exceptions.ROSException: self.logger.warning("timed out while waiting for tdma status")